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I. INTRODUCTION 


A. GENERAL BACKGROUND 

Military applications of autonomous vehicles are twofold: to delegate 
certain repetitive tasks to machines, allowing more efficient use of scarce 
human resources, and to avoid exposing humans to hazardous duties that 
could be satisfactorily performed by automated machines. An autonomous 
vehicle could transport supplies from a rear area support facility to a front 
line unit. It could also perform surveillance and measurement functions in 
areas contaminated by chemical or radiological agents. To accomplish these 
tasks, a vehicle must be capable of examining terrain data and selecting a path 
which permits efficient travel from a start point to a given destination. 

This process begins with high-level path planning, where the vehicle 
may take into account terrain, weather, concealment, and exposure to hostile 
forces in selecting the general route to take. One way to model this task is by 


using the weighted region problem. 


B. THE WEIGHTED REGION PROBLEM 
1. Definition 
The weighted region problem accepts input from a two-dimensional 
Cartesian map which models terrain as convex polygonal regions. Each 
region is assigned a cost coefficient or weight which is the cost per unit 
distance travelled, relative to other regions in the map. The cost of traversing 
a region is calculated by multiplying the region's weight by the Euclidean 


distance travelled through the region. See Figure 1. 





Figure 1. Weighted Region Problem 


Given a start point and destination point in terms of map 
coordinates, the goal is to find the most efficient path from start to destination 


through the weighted regions. 


An extension of the weighted region problem involves finding a path 
through sloped regions, where a ground vehicle's direction of travel is 
limited by its ability to climb steep hills and travel across sideslopes. 

Researchers describe this path planning problem as the anisotropic 
weighted region problem. Anisotropic means that the direction of travel also 
influences the cost of traversing a region. For example, it requires more 
energy to travel uphill on a path than it does to travel downhill on the same 
path. A heading which would cause a vehicle to exceed its center-of-gravity 
limits can be avoided by assigning an infinite (or sufficiently large) cost to this 
path. Because of the added dimension of elevation, this problem is 
sometimes referred to as 2; dimensional path planning. 

2. Problems to Overcome 

An efficient path planning program is not trivial. This program 
must select each segment of the path by comparing the surface condition and 
slope of each region with the physical limitations of the vehicle. A method of 
partitioning the terrain must be determined. One way is to divide the terrain 
into a grid such that each grid square possesses homogeneous terrain with 
respect to surface composition, slope, and traversal cost. While this method 
may be relatively simple to implement, a large number of grid squares 
requires large memory allocation and the solution path may not be safe due 
to the "stair stepping" characteristics of moving from one grid square to a 
neighboring grid square. 

In another technique, partitions are only made when necessary. A 
region is defined by homogeneous terrain, and is independent of size or 


shape. This approach reduces memory requirements, but complicates the 


search for the optimal path because the traditional "shortest path" graph 
search algorithms do not provide sufficient resolution to obtain an efficient 
path. Most implementations of this technique restrict the regions to convex 
polygons which simplifies the search and cost-calculating processes. 

The vehicle must avoid areas where its power or stability limitations 
are exceeded. If not, the vehicle will find itself facing a hill which it cannot 
climb or a sideslope which will cause it to overturn. Paths which lead 
downhill may also be undesirable. If the destination's elevation is equal to or 
higher than the start point's elevation, then travelling downhill only 
increases the net elevation the vehicle must climb in order to reach its 
destination. 

Once the program eliminates non-negotiable terrain regions from 
consideration, it must examine the remaining regions and select an acceptable 
path. These remaining regions may or may not be traversable, depending 
upon direction of travel. Again, the vehicle must avoid path headings that 
lead to excessively steep climbs or a potential rollover condition. 

The calculations required to conduct an exhaustive search for an 
optimal path in 2; dimensional space grow exponentially with the number of 
regions in the search space. Accuracy is a function of resolution of the search 
space. Greater accuracy requires smaller grids or regions that provide a more 
accurate representation of the terrain, but increase the number of calculations 
necessary to investigate each segment of the search space (or map). This 
computational workload requires the use of a computer to speed up the 


process and determine the results in a reasonable amount of time. 


Even with the aid of a computer, the time required to determine an 
optimal path may be excessive. Current algorithms do not find an optimal 


path through 2; dimensional space within acceptable time limits. 


Il. PREVIOUS WORK 


A. ALGORITHMS 

Several algorithms have been developed to solve the weighted region 
problem, including wavefront propagation [Ref. 1] and the continuous 
Dijkstra algorithm [Ref. 2]. The algorithms most impacting on this work are 


the systematic search by Ross and the stochastic approach by Kind. 


B. SYSTEMATIC SEARCH 

Ross [Ref. 3] describes an algorithm which finds an optimal path through 
anisotropic weighted regions. He implemented his work with the Common 
Lisp programming language. Ross's algorithm used a map that was divided 
into homogeneous regions. The area within a region shares a common slope, 
orientation, surface composition and condition, stability and braking 
constraints. 

He also defined a vehicle model which represents slope, climb, stability, 
and braking limits. He extensively described the trigonometric and physics 
equations used to determine which headings were allowable in each region. 

Ross used an A* search algorithm to find feasible paths to investigate. He 
calls a sequence of regions that contains a feasible path from start to goal a 
window sequence. See Figure 2. For each feasible window sequence, the 
algorithm conducts an exhaustive search of all paths that lie within the 
permissible heading ranges. 

Although this procedure finds the globally optimal path, the time 


required to find it renders the program impractical. Test results show it may 


require as much as 29 minutes to calculate a globally optimal path on a simple 


map containing 40 regions. 


Figure 2. Window Sequence 


C. STOCHASTIC APPROACH 

Kind] [Ref. 4] introduced a stochastic approach to solving the isotropic 
weighted region problem. He implemented his work using the C++ and 
Prolog languages. His approach is based on a technique called simulated 
annealing. Like Ross, Kind] defined the regions on a map by edges and 
vertices. Kind] placed a node at the mid-point of each edge and defined a 
series of arcs which connect every node in a region. He calls this structure an 


edge dual graph. See Figure 3. Kindl then finds a feasible window sequence 


by performing an A* search on the edge dual graph. This path is the basis for 
finding a locally optimal path through the generated window sequence. The 


locally optimal path is found by iteratively applying golden ratio search to the 
path segments. 


{ 
\ 
\ 
\ 
| 
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Figure 3. Edge Dual Graph 


Kindl's program then uses path annealing to perturb the window 
sequence by randomly re-routing path sequences through neighboring 
regions in an attempt to find a better path. 

Eligible neighboring regions are limited by a bounding ellipse which is an 
oval drawn around the start and goal. Regions or portions of regions outside 
the bounding ellipse are not considered by the annealing process. 

While not guaranteed to find the globally optimal path, this procedure 
does find a near optimal path in much less time. Although Kindl's 
procedure produced excellent results, its scope is limited to two dimensional 
path planning. Extension of Kindl's work to 2; dimensional path planning 


will significantly broaden its applications. 


III. GENERAL PROBLEM-SOLVING STRATEGY 


To solve the anisotropic weighted region problem, we need vehicle and 
terrain models, geometric spatial reasoning functions, search functions, cost 
functions, and annealing control functions. 


Given a vehicle and map, the general strategy is: 


1. perform a search through the map regions to create a region list which 
may contain an initial path solution. 


2. calculate necessary information and put these regions into a data 
structure called window list. 


3. for each adjacent pair of regions in the window list, iteratively 
(a) determine heading ranges and cost functions. 


(b) perform a golden ratio search on each heading range combination 
and locate the optimal heading range combination. 


(c) perform a golden ratio search on the region crossing frontier 
defined by the optimal heading range combination and locate 
(within tolerance) the optimal crossing point. 


4. if any crossing point shifted more than the specified displacement, go 
to 3. 


5. if any path segment headings are impermissible, attempt to detour 
around them to find a valid path segment. 


6. use random annealing in an attempt to find a shorter path. 


These steps are described in greater detail in subsequent chapters. 
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IV. MODELING THE VEHICLE AND TERRAIN 


A. VEHICLE 

The vehicle model used in this work is a simplified version of those used 
by Ross and Rowe [Ref. 5] and Rowe and Kanayama [Ref. 6]. We are 
concerned with vehicle-specific heading limits for climbing, braking, and 
slideslope travel. See Figure 4. Given a vehicle’s heading limits and the 
slope and orientation of a region, critical headings can be computed by the 


following formulas: 


critical-p = arccos ((tan limit-p) / (tan slope)) 
critical-s = arcsin ((tan limit-s) / (tan slope)) 
critical-b = arccos ((tan limit-b) / (tan slope)) 


where limit-p, limit-s, limit-b are, respectively, the slope angle thresholds 
where the vehicle's uphill, sideslope, and braking limits occur. 
Each critical angle has a dual, which with the critical angle defines a 


heading range. These dual headings are calculated as follows: 


dual-p = - critical-p 
dual-s = & - critical-s 
dual-b = - critical-b 
critical-l = - critical-s 


dual-l = critical-s - x 
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power limit 
heading range 







braking 
heading range 





gravity 
(a) Rollover condition 
(b) Sideslope safe condition 


Figure 4. Vehicle Model 


There are four ranges: power-limited, braking, and two sideslope ranges, 


one to the vehicle's left and the other to the right. If the vehicle's heading 


2 


falls within one of these ranges, the corresponding condition applies. See 
Figure 5. For example, if the critical power-limit angle is 20° and its dual is 
340°, a vehicle travelling at a heading of 355° is subject to the power-limited 


condition. 


dual-power 
heading critical-power 


power-limited heading 
range 


critical-right 
heading 


critical-left 
heading 


side slope side slope 
limited limited 


ran HH range 


dual-left dual-right 
heading heading 


dual-braking critical-braking 
heading heading 





Figure 5. Critical heading ranges 
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These critical headings will vary from region to region as the slope varies. 
If a critical heading is equal to its dual, the region's slope is less than the 
vehicle's heading limit and no heading restriction exists for that particular 
range. For example, if a region's critical sideslope heading is equal to its dual 
sideslope heading, the vehicle can freely traverse the region without fear of 


rolling over. 


B. TERRAIN 


1. Homogeneous Regions 
The terrain is modelled by convex homogeneous polygonal regions. 
Regions boundaries are defined by vertices and each region possesses uniform 
surface condition, slope and weight. The maps we used were manually 
constructed and do not represent actual terrain. See Figure 6. 
2. Vertex List 
A vertex is a point defined by x, y, and z coordinates. A vertex can be 
on the boundary of more than one region. All vertices necessary to define the 
map are collected into a vertex list. 
3. Region List 
The region list is a collection of all regions in the map and contains 
the vertex list, weight, slope, orientation, and adjacent region list for each 
region. 
4. Frontier 
A frontier is the line separating two regions. It is represented as an 


edge with its endpoints defined by vertices. 
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vertices 7 a nr e- 


frontier 


region 1 region 2 


Region 1 is defined by vertex list (V1, V2, V5, V6) 
Region 2 is defined by vertex list (V2, V3, V6, V5) 
The frontier between region 1 and region 2 is defined by edge (V5 V2) 





Figure 6. Regions, Vertices, and Frontiers 


4. Window list 

Once an initial window sequence (a list of regions) is computed, each 
region in the list is put into the window list (a linked list). Additional 
information for the window list (such as all critical headings, and various 
"housekeeping" variables) is now calculated. We only compute these values 
for regions in the window list, and thus, only for the regions we will actually 
“visit. " | 
5. Typel, II, Il, lV Traversals 


Ross classified traversals through a region into four types: 
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A Type I traversal is the simplest. It is merely travel from one 
isotropic region to another isotropic region. In other words, the crossing 
point of the boundary edge is determined with respect to weighted distance 
only. There are no heading restrictions associated with this type of crossing. 

With a Type II traversal the optimal path must be altered in one or 
both regions because it falls within a nonpermissible sideslope heading range. 
The nonpermissible heading is adjusted to the nearest critical angle, which 
allows the path to lie as close as possible to the optimal (but forbidden) 
heading. 

Type III traversals feature switchbacks, which allow a vehicle to travel 
through an uphill nonpermissible heading range by switching between the 
pair of critical angles that border the nonpermissible range. The vehicle is 
allowed to make as many switchbacks as necessary to traverse the region. 

Type IV involves travelling downslope where braking is required. 
There are no heading restrictions, but a different cost function (described in 
the next section) is required for travel within the braking range. See Figure 7. 

7. Cost Functions 

For Type I traversals, the cost function is simply the region's weight 
multiplied by the Euclidean distance calculated from the point of entry, p1, to 
the point of exit, p2. 

cl = weight x distance (p1, p2) 
For Type II traversals, the cost function is also weighted distance, as in 


Type I. However, recall that the heading is adjusted to a critical heading to 


allow passage through the region. 


c2 = weight x distance (p1, p2) 
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Figure 7. Type I, II, III, lV Traversals 
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Calculating Type HI traversals also involves a weighted distance cost 
function, but the distance must take into account switchbacks. The distance 
for a switchback traversal, regardless of the number of switchbacks 
encountered, requires calculating the intersection of two rays. The first ray 
originates at start point p1 of the first segment, sw1, with a direction of the 
critical impermissible heading. The second ray originates at the start point p2 
of the second segment, sw2, with a direction of m + the dual critical 
nonpermissible heading. 

Call this point of intersection i. The cost is the weighted distance 
from the start point of the first segment to the point of intersection i plus the 
weighted distance from the point of intersection i to the start point of the 


second segment. See Figure 8. 


c3 = weight x distance (p1, i) + distance (i, p2) 


The cost function for Type IV (braking) traversals must take into 
account changes in elevation. Although our vehicle model claims no energy 
is expended while travelling within a braking region, a virtual cost is 
incurred because potential energy is lost as a result of the vehicle's decrease in 
elevation [Ref. 6]. The cost for travelling within a braking region is thus 
calculated as the change in elevation from the point of entering the region, 
pi, to the point of exiting the region, p2. Distance is not relevant to this cost 
function. 


c4 = abs ((elevation p1) - (elevation p2)) 
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Figure 8. Finding a Switchback Point 


V. PROBLEM SOLVING 


A. A* SEARCH 


1. Edge Dual Graph 

Kindl's edge dual graph provides an excellent means of locating 
feasible window sequences. The fully connected edge mid-points (nodes) of a 
region provide a good cost estimate of traversing a particular portion of the 
region. See Figure 9. But instead of constructing a complete edge dual graph, 
we construct "on the fly." We only compute distances for the region currently 
being expanded. 

At this point of the search we do not take into account vehicle 
heading restrictions (except as noted in the next paragraph), but we do expect 
every region in the sequence to be traversable within at least one more 
heading range. The A* search also avoids traversal of narrow regions where 
all headings between two frontiers fall within a non-permissible slideslope 
range. See Figure 10. Other regions may be labelled as obstacles if their 
traversal costs exceed a pre-determined threshold value. The window 
sequence returned by this search procedure is not guaranteed to be feasible. 
Although any single region in the sequence may be traversable, a crossing 
point between two regions which simultaneously avoids nonpermissible 
sideslope headings in both regions may not exist. See Figure 11. In this case, 


we attempt to detour around the impermissible path segment. 
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It is impossible to travel from the entry frontier to the exit 
frontier without violating the sideslope heading restrictions. 


Figure 10. Narrow Region with Nonpermissible Entry 


Pip 


region 2 





Figure 11. Two Regions with no Valid Crossing Point 


2. Heuristic 
The heuristic evaluation function for the A* search is the shortest 
distance to the goal. The distance from each active node (midpoint of the 
edge) to the destination is calculated and added to the cumulative cost of 


reaching the node. The node with the lowest sum is selected for expansion. 


B. FINDING THE LOCALLY OPTIMAL PATH 


1. Processing a Window Sequence 
The A* search returns a list of regions within which a feasible path 


lies. The region list, start, and goal are processed into a linked list which 
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contains region-specific information such as critical heading ranges, boundary 
edge and exit point, and a displacement which measures the magnitude of a 


point's movement during the iterative optimization process. 


2. Golden Ratio Search 

The golden ratio search is used to approximate the locally optimal 
path through two adjacent regions. The procedure involves two path 
segments, with one segment for each region being searched. The outer end 
points of the path segments remain fixed, and the interior point where the 
two segments meet is adjusted along the frontier (within a given tolerance) to 
find the minimum cost. See Figure 12. 

The procedure exploits the convex function defined by adjusting the 
crossing point along the frontier. By sampling four points along the frontier, 
it is possible to determine that the optimal cost lies within the range of three 
of these points and the search space beyond these three points can be 
eliminated from the search process. This procedure can be repeated on an 
increasingly smaller search space until the desired accuracy is obtained. 
However, the procedure does reach a point of diminishing returns where 
additional searching produces negligible results. We halt the search when 
improvement is less than two percent of the current path cost 

3. Partitioning the Regions by Heading Ranges. 

When calculating the cost of traversing adjacent regions, we must 
determine within which heading range the vehicle is travelling... The 
vehicle's heading determines which cost function to use. There are three 


permissible heading ranges: headings within the power range, braking range, 
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min (p1 p2 p3 p4) = p3, therefore the global 
minimum cannot lie on 
the curve between p1 and p2 





Figure 12. Golden Ratio Search 


and isotropic (unrestricted) range. We must also consider the nonpermissible 
sideslope heading ranges because an initial crossing point may force a path 
segment to lie within a nonpermissible heading range. Since the golden ratio 
search requires a convex cost function to approximate the cost of traversing 
two regions, we must consider the possible combinations of heading ranges 
(and thus cost functions) between two adjacent regions. There are 42 = 16 


possible combinations. See Table 1. 


TABLE 1. HEADING RANGE COMBINATIONS 


| power | isotropic 
isotropic | ip | it | 
sidestope | sp | sit | 


For example, “ip” represents the cost function for traversing a region within 
















an isotropic heading range and transitioning to a power heading range when 
crossing into the next region. The ranges involving nonpermissible sideslope 
headings are assigned a large value representing “infinity.” 

We distinguish these heading range combinations by computing 
transition points along the frontier. Each transition point marks the 
transition from one heading range (and thus cost function) to another. See 
Figure 13. 

The end points of the frontier serve as the initial and final transition 
points. We must also keep track of which region (the region before the 
frontier or after the frontier) and heading range the transition point is 
associated with. We now find intermediate transition points along the 
frontier, if any exist. For the first region, we compute a ray from the region’s 
entry point at all eight critical headings. For every ray that intersects the 
frontier, we assign a transition point at the point of intersection. For the 
second region (beyond the frontier), we compute "back rays" from the 
region's exit point at each critical heading + m. We again assign a transition 


point where any back ray intersects the frontier. 
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Figure 13. Partitioning Regions by Heading Ranges 


Working from left to right along the frontier, we conduct a series of 
golden ratio searches. Each time we encounter a transition point, we change 


to the appropriate cost function. 


oT 


After one sweep of the frontier, we have determined which heading 
range combination is likely to contain the optimal crossing point. Since we 
know between which two transition points this optimal crossing point lies 
(and thus the appropriate cost function to apply), we conduct a more precise 
golden ratio search along the frontier between these two transition points. 
This search locates (within tolerance) the optimal crossing point between the 


two regions. 


C SIMULATED ANNEALING 


1. Background 
Johnson, et al, [Ref. 7], describe the use of simulated annealing for 
solving the graph partitioning problem. This is essentially a modified 
technique of iterative improvement of a local solution until an acceptable 
result is achieved. Annealing allows for occasional acceptance of a 
degenerative solution in an attempt to escape from a locally optimal but 
globally poor solution. 
The basic simulated annealing algorithm requires as input: 
1) the solution space 
2) control parameters for the annealing process, which include 
a) To - the initial value of the control temperature T 
b) Tr - the "freezing" value of T 
c) KR - the reduction factor for T (typically 0.70 <= R <= 0.99) 
d) L - the maximum number of attempted moves at each value of T 
e) Ls - the maximum number of accepted moves at each value of T 
Beginning with the initial solution, the move generator randomly 


perturbs this solution to obtain a new one. The change in cost, Ac, is the 
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difference between the cost of the new solution and the current solution. If 
Ac < 0, the new cost is less than the current cost, and the new solution 
becomes the current solution. If Ac > 0, the new solution is accepted as the 


current solution based on a probability function. The typical probability 
mee 
function is P=e | , where Ac is the cost difference and T is the current 


temperature. 

The control temperature T is expressed in the same units as the cost 
function. As the solution space is searched, T is reduced by the reduction 
factor R which progressively reduces the probability of accepting a solution 
with higher cost. 

At each temperature T, the move generator attempts up to L moves, 
accepting up to L, solutions of higher cost. The temperature eventually 
reduces to Ty, the freezing temperature, where no solutions with higher costs 
are accepted. At this point the algorithm halts and returns the best solution 
found. 

Kindl [Refs. 4, 8] used this annealing concept in attempting to 
improve a current path solution. Recall that a path lies in a window 
sequence. The border between windows is an edge whose endpoints are 
defined by vertices. A vertex may define the endpoint of more than one edge. 
Thus, a vertex may be common to more than one window (or region). 

Kindl's procedure randomly selects a vertex, called the rotation 
vertex, which belongs to the current window sequence. All edges with this 
vertex as an endpoint are identified. The current path solution passes 
through at least one of these edges. Edges through which the path crosses are 


removed from the window sequence and replaced by the edges through 


Zo 


which the path does not pass. See Figure 14. This generates a path through 


the new window sequence which is iteratively improved (using the golden 






rotation 
vertex 
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Figure 14. Rotation Vertex 


ratio search) to achieve a locally optimal solution. If this path solution is 


better than the previous solution, the new path is accepted. If the new path 


solution is worse than the previous solution, the decision to accept it is 
—Ac 


determined by the probability function P=e ' . This allows a means of 
escaping from a possible local minimum and searching other areas of the 
map for a better solution. See Figure 15. Kindl used the following control 


parameters: 


To = initial path cost 


nm = 1.0 
= .80 

= 

L, =4 


local 
minimum 
local 
minimum 


global minimum 





Figure 15. The Cost Function for Multiple Window Sequences 


2. Path Annealing for Anisotropic Regions 
The control mechanism Kind] used will also work for anisotropic 
path annealing. Temperature will control the program's probability of 


accepting worse or degenerative solutions. 


31 


The process which perturbs the path, or makes a "detour," is more 
complicated. A simple replacement of edges based on a rotation vertex may 
not produce a feasible window sequence. We must check the new edges to 
determine whether we can travel between them at a valid heading. 

To do this, we select a detour region. We designate the entry point 
into the detour region as the detour start point. We assign the exit frontier of 
the detour region as the detour frontier. The exit point of the region 
immediately beyond the detour region is assigned as the detour goal point. 
We now conduct an A* search similar to the search performed to produce the 
initial window list. The search begins at the detour start point and terminates 
under one of two conditions: 1) the detour goal point is reached, or 2) an 
upstream region is reached. An upstream region is a region in the window 
list between the detour region and the (global) goal region. See Figure 16. 

For the A* search we use frontier endpoints in addition to frontier 
midpoints as nodes. This gives us greater resolution and often results in a 
shorter detour. This procedure is particularly useful when searching across 
regions that vary greatly in width. 

This A* search is more restrictive than the search used to find the 
initial region list. In addition to checking for valid heading ranges between 
regions, we limit the length of the detour to no more than three regions. 
This "tames" the search and prevents it from producing a circuitous detour 
which would likely result in a path solution considerably longer than the 


current solution. 


OZ 


detour-goal 


detour-edge 





Figure 16. Finding a Detour 


This procedure effectively eliminates many regions as candidate 
detour regions. This is desirable, since we don't want to waste our time on 
detours unlikely to produce a better path solution. 

The A* search produces a detour region list. The detour region list is 
processed into a window list and inserted into the current window list, 


replacing the detour region(s). The resulting window list will contain at least 
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one different region where the detour occurs, but it may contain up to two 
new regions. 

The golden ratio search is iteratively applied to the new window list 
to determine a locally optimal (within tolerance) path solution. The frontier 
crossing points of the windows retained from the previous window list serve 
as a Starting solution for this next search. The result is a faster search, since a 
locally optimal solution already exists for path segments before and beyond 
the detour region(s). 

Once the locally optimal path is found, we compare it to the current 
best path. If the current path is shorter, it becomes the best path. If the 
current path is longer than the best path and not accepted by the annealing 
control mechanism, the previous window list is restored and a new region is 
randomly selected for a detour. 

The number of detours attempted is determined by a loop structure 
and the preset value L, and the maximum number of accepted moves any a 
given temperature is Lg. 


We initially use the probability function mentioned previously, 
—-AC 


P=e T . We use Kindl’s values for control parameters, except for R, where 
we use .70. 

Since our relatively simple map provides few opportunities to exploit 
path annealing, we also experiment with a linear probability function 
P = T. The number of detour attempts allowed is a function of the length of 
the window list. We also discard the value Ls, which governs the maximum 


number of accepted moves at a given value of T. The temperature T is 
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reduced by a constant R (i.e. T = T — R) after every L detour attempts. The 
results are compared in Chapter VI. 

We always keep track of the current path solution and the best path 
solution. We also maintain a path list. A new path solution is added to the 
path list if it is accepted as the current solution. The path list allows us to 
provide alternate path solutions which may be longer than the best path, but 


are nevertheless feasible path solutions. 
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VI. EMPIRICAL STUDIES 


A. INPUT MAPS AND VEHICLES 
1. Maps 

Our input map, derived from Ross, is not particularly suited to 
exploiting the annealing process. The regions are largely symmetrical and 
oblong. This arrangement often permits the A* search to produce the 
optimal window list instead of relying on the annealing process to find it. In 
this case, most of the effort is spent in merely confirming that we have 
already located the best path. However, there are some cases where annealing 
produces a better solution. In our test cases, we display the path solution 
produced by the A* search as well as the solution found by annealing. 

We use two maps. The maps appear identical, but the second map 
contains regions weighted in proportion to their slopes. All regions in the 
first map have a weight of 1. Figures 17 and 18 are included to show map 
features. Figure 17 shows a path found by the A* search. Figure 18 shows the 
path solution after annealing. Heading range indicators for the vehicle are 
drawn at each point where the path crosses a frontier. 

2. Wehicle 

We use three vehicles for the evaluation. Vehicle-1 and vehicle-3 are 
adapted from Ross [Ref. 1] and represent, respectively, an armored personnel 
carrier and a cargo truck. Vehicle-2 cannot climb as steep a slope as vehicle-1, 
but its sideslope stability is greater. We did this to see how much the path 


between a common start and goal varies for different vehicles. 
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Figure 17. Path before Annealing 
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Figure 18. Path after Annealing 
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B. TEST PROCEDURES 

A direct comparison of our implementation to Ross's is not possible since 
Ross used Flavors on a Symbolics LISP machine while we use Allegro 
Common LISP on a Solbourne workstation. We are not certain whether 
Ross's time computations include all overhead, output, and display 
procedures. Also, our map is slightly modified from Ross's. The slope of 
some regions was changed to be more consistent with vertex elevations. All 
regions in Ross's map are equally weighted, therefore a comparison of our 
performance with the weighted map would be invalid. 

We do observe the performance of our implementation based on 
different vehicles, temperature cooling schedules, and maps. For our time 
computation (msec), we include all processes, beginning with input of the 
start and goal points. We also include time allocated to updating displays and 
input/output. 


We evaluate the path annealing process with two different probability 
—-AC 
functions, the cost-based exponential function P=e T , and the linearly 


decreasing function P = T. Table 2 shows the results of this comparison for 
vehicle-1 and Table 3 shows the results for vehicle-2. The temperature and 
the initial path cost in the exponential function influences the number of 
detours attempted as well as the probability of accepting a degenerative path 
solution. In the linearly decreasing function, the number of detours 
attempted is dependent on length of the window list and the probability of 


accepting a degenerative path solution begins with a constant initial 
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temperature and decreases at a constant rate during each iteration until it 


reaches the freezing temperature. 


TABLE 2. EQUALLY WEIGHTED REGIONS WITH VEHICLE-1 


_—_siset_}__as__}_tz exponciill 


(221.0 399.0) (260.0 193.0) ; 0 107348 4 159951 
(229.0 127.0) (305.0 344.0) 8 34284 7 40784 
(399.0 217.0) (561.0 239.0) 4 41599 4 64667 
(271.0 415.0) (266.0 194.0) , 1 58517 1 87466 
(77.0 339.0) (364.0 297.0) 8 100617 8 144850 


399.0) (260.0 193.0) 109.7 82200 7 217533 


127.0) (305.0 344.0) : 20600 | 156.3 39366 3 115684 
217.0) (561.0 239.0) 17434 51.15 103717 2 107083 
415.0) (266.0 194.0) : 21366 | 176.7 39700 ' 88450 





339.0) (364.0 297.0) 24199 | 217.9 63501 8 179266 
We also tested the annealing process on the weighted region map. The 


results are shown in Tables 4 and 5. 
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TABLE 4. WEIGHT PROPORTIONAL TO SLOPES WITH VEHICLE-1 


at | tinearp | exponentiat p 
| sat goat cost time | cost _time | cost _ time _ 
(221.0 399.0) (260.0 193.0) 201034 
(229.0 127.0) (305.0 344.0) : 30100 
(399.0 217.0) (561.0 239.0) 52066 
(271.0 415.0) (266.0 194.0) 69000 
(77.0 339.0) (364.0 297.0) ; 46949 

































TABLE 5. WEIGHT PROPORTIONAL TO SLOPE WITH VEHICLE-2 


_tinearp | exponential p_ 
cost time | cost time _ 
158816 


(221.0 399.0) (260.0 193.0) 30449 

(229.0 127.0) (305.0 344.0) 10533 35384 
(399.0 217.0) (561.0 239.0) 38233 
(271.0 415.0) (266.0 194.0) 20966 
(77.0 339.0) (364.0 297.0) 20617 


















44634 


29784 
46234 





C ANALYSIS OF TEST RESULTS. 


The results support the conclusion that path annealing can improve 
upon an initial A* search solution in the 2 dimensional weighted region 
problem. However, the stochastic approach may possess disadvantages. 
Because detours are chosen randomly, we are not guaranteed of achieving the 
same solution for multiple runs of the program. Even if multiple runs 
produce the same solution, we don't know when the best solution was found. 
It may be found on the first detour, or it may be found only after making 


many detours. 
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If time is limited, we should be able to halt the program prematurely and 
return the current best solution. The randomness of choosing a detour 
region virtually assures that a program repeatedly halted at the same point 
will return different current solutions for the same start and goal. 

A possible cure for this is to select a detour region based on a heuristic 
evaluation. We should attempt to detour through regions with a high 
potential for reducing the current path solution. A relatively high cost region 
or a path segment with a sharp change in direction may be promising 
candidates for a detour. This area warrants further study. 

In our test cases, the linearly decreasing probability function works as well 
as the exponential probability function as far as finding the best path. 
Execution time is less for the former, however, this observation is based on 
very limited testing and is probably dependent on the shapes and sizes of map 


regions. 
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VII. SUMMARY AND CONCLUSIONS 


Finding the optimal path solution through complex and varied terrain is 
expensive in terms of time and computer resources. Often heuristics can be 
applied to find a near-optimal or otherwise acceptable path solution in much 
less time. Several efforts have been made to locate feasible paths through 
two-dimensional terrain. This work expands Kindl's stochastic path 
planning approach to the 25 dimensional weighted region problem. 

It is important to begin with a good initial window list. This is 
accomplished by an A* search. The effectiveness of path annealing is 
probably dependent on characteristics of the terrain, but further study is 
needed in this area. Random annealing works, but a heuristic approach to 
annealing should be explored. For example, it may prove beneficial to detour 
around high cost regions. 

To be certain of finding the globally optimal solution, one must 
exhaustively search every feasible window sequence. Ross shows that this 
can be prohibitively expensive in terms of time and computational effort. A 
heuristic and stochastic approach produces a timely and acceptable path 
solution, but random detours will not always return the same path solution 
for a given start and goal. 

The basic concept is shown to work in the LISP implementation. We do 
not guarantee an optimal or near-optimal path solution will be found. We 
do begin with a feasible starting solution and iteratiavely attempt to improve 
it under the control of the golden ratio search tolerances and the annealing 


temperature. 
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Suggested extensions to this research include testing and evaluating the 
performance of this implementation on various terrain models, 
incorporating a terrain mapping program, developing heuristic evaluation 
functions for selecting detour regions, and the use of parallel processing to 


simultaneously compute multiple window sequences. 


LIST OF REFERENCES 


Richbourg, R. F. , Solving a Class of Spatial Reasoning Problems: 
Minimal-Cost Path Planning in the Cartesian Plane, Ph. D. Dissertation, 
Computer Science Department, Naval Postgraduate School, Monterey, 
California, June 1987. 


Mitchell, J. S. B. and Papadimtriiou, C. H., “The Weighted Region 
Problem: Finding Shortest Paths Through a Weighted Planar 
Subdivision,” Journal of the ACM, 1990. 


Ross, R. S. , Planning Minimum-Energy Paths in a _ Off-Road 
Environment with Anisotropic Traversal Costs and Motion Constraints, 
Ph. D. Dissertation, Naval Postgraduate School, Monterey, California, 
June 1989. 


Kindl, M. R., A Stochastic Approach to Path Planning in the Weighted 
Region Problem, Ph. D. Dissertation, Naval Postgraduate School, 
Monterey, California, March 1991. 


Ross, R. S. and Rowe, N. C. , Optimal Grid-Free Path Planning Across 
Arbitrarily Contoured Terrain with Anisotropic Friction and Gravity 
Effects, Naval Postgraduate School, Monterey, California. 


Rowe, N. C. and Kanayama, Y. , Minimum-Energy Paths on a Vertical- 
Axis Cone with Anisotropic Friction and Gravity Effects, Naval 
Postgraduate School, Monterey, California. 


Johnson, D. S. , Aragon, C. R. , McGeoch, L. A. and Schevon, C. , 
Optimization by Simulated Annealing: An Experimental Evaluation; Part 
I, Graph Partitioning,” Operations Research, v. 37, no. 6, November- 
December 1989. 


Kindl, M. R. , Rowe, N. C. and Shing, M. , Solving the Weighted Region 


Problem with Simulated Annealing, Naval Postgraduate School, 
Monterey, California. 


45 


APPENDIX A. NOTES ON THE IMPLEMENTATION 


The implementation program was written in Common LISP with X 
Windows and run on a Solburne Workstation. The system possesses 
its Own operating system, 16 Megabytes of RAM, and runs at a clock 
speed of 33 MHz. 


The emphasis was on producing a working prototype with efficiency a 
secondary priority. Many of the routines used in this program were 
more efficiently implemented using C++ and Prolog by Kindl. We 
perform very little pre-processing on the map data, preferring instead 
to calculate as we conduct the search. In an application where one map 
is extensively used, pre-processing would undoubtedly prove more 
efficient. 


We used only the simplest data structures. The program is essentially 
constructed with LISP's defstruct, cond, if, dolist, cons, append 
functions. Sequential doubly linked lists were used instead of arrays, 
hash tables, or binary trees. The intent was to produce simple and 
readable code (as far as this is possible in LISP). 


Local variables were primarily used. We only used global variables for 
such data as the start/goal points, window lists, region lists, accuracy 
control variables, and annealing control variables. 


There is no elaborate user interface. The user can use the setf 
command to change vehicles. The start and goal points are input by 
using the mouse. 


APPENDIX B . SOURCE CODE (COMMON LISP) 


KKK KKK KKK KKK KKK KK KKK KKK KKK KKK KKK KK KK KKK KKK KKK KKK KK KKK KEK KEK KK KKKKKK 


Filename: start.cl 


By: HILTON, Cary A. 


Date: May 91 


i 
2, 
oe 


creates and activates window *map* 
Heads file “find.cl" 
to begin, type "(path)", then 


click left mouse button on 
start, goal points 


meesnceion: startup file for anisotropic path planning. 


CK KKK KKK KKK KKK KKK KKK KKK KEKE KKK KK KKK KEK KEK KEK KKK KK KKK KKK KKK KE KEKEKKKKEKKKK 


a 


(use-package 
(require :xcw) 


(initialize-common-windows) 


(defvar *map*) 


(defvar *first-win*) 


(defvar *win-num*) 


(defvar *x-min* 0) 


; (defvar *x-max* 695) 
(defvar *x-max* 600) 
(defvar *y-min* 25) 

; (defvar *y-max* 838) 
(defvar *y-max* 830) 


(setf£ *map* 


(make-window-stream 
ret t *x—min* 


sbottom *y-min* 

:width *x—max* 

sheight *y-max* 

7c Lt Le "MAP WINDOW") ) 
(activate *map*) 


(load “find.cw™®) 
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Filename: find.cl 


By: 


Fun 


okxKKKK 


, 


(load 
(load 
(load 
(load 
(load 
(load 
(load 
(load 
(load 
(load 


; Date: May 91 


HILTON, Cary A. 


ction: Control structures for aregram: 
1. loads all files necessary 
to run program (except “start.cl") 
2. sets default maps and vehicles 
3. contains top level control program 
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"anneal.cl") 
"converge.cl") 
NGOStL Cl) 
"Wdira wh sei! } 
"geometry.cl") 
links. cll) 
“partition.c!.) 
"search.cl") 
"vehicle.cl") 
"map2.cl") 


; commands to load compiled files 


; (load 
; (load 
; (load 
>(1cad 
; (load 
3; (load 
; (load 
; (Load 
; (load 


; (load 


pewkkr 


"“anneal.fasl") 
"converge. fasl") 
“costi.fasil") 
"“drawf.fasl") 
"geometry.fasl") 
"links.fasl") 
“partition. fasl") 
"search.fasl") 
"vehicle.fasl") 


"map2.fasl") 
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(defstruct path 
region 
point 
heading 
cost) 


(defvar *start*) 
(defvar *goal*) 
(defvar *rlist*) 
(defvar *winlist*) 
(defvar *init-win*) 


global start point 

global goal-point 

path passes through region list 
processed window list 

first window in window list 


swe “Ye “Ye Se Veo 
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(defvar *first-win*) ; second window in window list 


(defvar *path-list* nil) 
(defvar *current-path* nil) 
(defvar *best-path* nil) 


(defvar *new-path* nil) 

(defvar *random-value*) 

(defvar *accept-path* .5) ; this is the annealing "temperature" 
; when temperature reaches 0, no 
; degenerate paths will be accepted 


(defvar *l-move* 4) 

(defvar *l-accept* 4) 

(defvar *reduction-factor* 0.7) 
(defvar *temp-f* 1.0) 
(defconstant *e* 2.71828) 
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; Default vehicle is vehicle-1l 

(setf *vehicle* (vehicle-rad vehicle-1) ) 
;(setf£ *vehicle* (vehicle-rad vehicle-2) ) 
;(setf *vehicle* (vehicle-rad vehicle-3) ) 
mitoad “map3.cl") 


; (load "map3.fasl") 
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(defun get-start () 
(let (mouse-p) 
(pprint "click mouse on start point") 
(setf mouse-p (get-position *map*) ) 
(setf *start* (cons (float (position-x mouse-p) ) 
(list (float (position-y mouse-p))))) 
(draw-start *start*) 
(Pprint *“start~)) ) 


(defun get-goal () 
(let (mouse-p) 
(pprint “click mouse on goal point") 
(setf mouse-p (get-position *map*) ) 
(setf£ *goal* (cons (float (position-x mouse-p) ) 
(list (float (position-y mouse-p))))) 
(draw-goal *goal*) 
(pprint *goal*))) 


(defun create-window-list (start goal) 
(setf *winlist* (change-struct start goal *rlist*)) 
(double-link (first *winlist*) (rest *winlist*) ) 
(setf *init-win* (first *winlist*)) 
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(setf *first-win* (segment-next-win *init-win*) ) 
(setf *win-num* (segment-index (first (last *winlist*))))) 


(defun path () 
(setf *best-path* nil) 
(setf *path-list* nil) 
(setf *current-path* nil) 
(clear *map*) 
(draw-regions *global-region-list*) 
; (get-start) 
;  (get-goal) 
(find-path *start* *goal*) 
(loop 
(cond (*bad-segments* 
(detour-at (first *bad-segments*) ) 
(qdraw) 
(converge *first-win*) )) 
: (draw-final *first-win*) 
(if (null *bad-segments*) 
(return) ) ) 
(setf *current-path* (store-path *first-win*) ) 
(setf *best-path* *current-path*) 
(setf *path-list* (append *path-list* 
(list *evrrent—path~))y) 
; (anneal-path) ; linear cooling 
; (anneal-path2) ; exponential cooling 
Tally 


(defun anneal-path () 


(let ((detour-region) (attempts) ; “attempts" is assigned the 
number 
(detour-count 0) (move-prob) ; of regions in the region list 
(visited) ) 
(loop 


(setf attempts (segment-index 
(first (last *winlist*)))) 
(setf detour-region (random-region) ) 
(pprint “detour-region:") (pprint detour-region) 
(unless (memberp detour-region visited) 
(detour-at detour-region) 
(cond (*detour-rlist* 
(qdraw) 
(converge *first-win*) 
(draw-final *first-win*) 
(setf£ *current-path* (store-patrh “firseewan 
(setf *accept-path* 
(—- *aecept=parhieo.)) 
(cond ((or (null *best-path*) 
(< (first *current-path*) 
(first *best-path*) )) 
(setf *best-path* *current-path*) 
(setf *path-list* (append *path-list* 
(list *current-—path*)))) 
Me 
(setf move-prob (/ (random 100) 100)) 
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(if (or (> (first *current-path*) 
Fini amet y* ) ) 
(> move-prob *accept-path*) ) 
(restore-path))))) 
(setf visited (append visited 
(list detour-region)))) ; end unless 
(incf detour-count) 
(1£f (>= detour-count attempts) 
(return))) ; end loop 
(draw-best-path *best-path*) 
(print-stored-costs *path-list*))) ; end let, defun 


(defun anneal-path2 () 
(let ((temp (first *best-path*) ) 
(accepted 0) (prob) 
(detour-region) (visited) 
(delta-c) (decision-point) ) 
(unless (> *temp-f£* temp) 
(loop 
(dotimes (index *l-move*) 
(setf£ detour-region (random-region) ) 
(unless (memberp detour-region visited) 
(detour-at detour-region) 
(cond (*detour-rlist* 
(qdraw) 
(converge *first-win*) 
(draw-final *first-win*) 
(setf *current-path* (store-path *first-win*) ) 
(set£ delta-c (- (first *current-path*) 
(first *best-path*) )) 
(cond ((and (< accepted *l-accept*) 
(< delta-c Q)) 
(incf accepted) 
(setf£ *path-list* 
(append *path-list* 
(Tistatbest—path*)),)) 
(setf *best-path* *current-path*) ) 
LG 
(set£ prob 
(expt *e* (- (/ delta-c temp)))) 
(setf£ decision-point (/ (random 100) 100)) 
(if (or (> (first *current-path*) 
cig eesl gohan.) 
(> decision-point prob) ) 
(eeceOuC Cat hjawes)) ; end cond 
(set£ visited (append visited 
(list detour-region))))) ; end unless, 
dotimes 
(set£f temp (* temp *reduction-factor*) ) 
(if (<= temp *temp-f*) 
(return)))) ; end loop, unless 
(draw-best-path *best-path*) 
(print-stored-costs *path-list*))) ;end let, defun 


(defun find-path (start goal) 


| 


(let ((start-region (find=-region *start*)) 
(goal-~region (find-region *goal*) ) 
(path-cost) ) 
(pprint "start-region: ") (pprint start-region) 
(pprint “goal-region: "™) (pprint goal-region) 
(cond ((eq start-region goal-region) 
(setf£ *ritst* (list. start-reqiueny) 
(pprint “*rlist*;” “) (ppranee: else 
(create-window-list start goal) 
(setf£ path-cost 
(check-direct-path start goal *first-win*) ) 
(unless (null path-cost) 
(setf (segment-cost *first-win*) 
path-cost) 
(pprint “heading: ") 
(pprint (rad-to-deg (angle (cons *start* 
(list *goal*))))) 
(Dprinty ‘patn—ecest . =} 
(pprint path-cost) 
(draw-direct *start* *goal* *first-win*)))) 


(cond ((null path-cose; 

(setf *rlist* (region~-search start goal 
start-region 
goal-region) ) 

(cond = (*rlise* 


(pprint “*rlist* apprise: Grice.) 
(create-window-list start goal) 
(qdraw) 


(converge *first-win*) 
(draw-final *first-win*) ) 
(t 
(pprint “no solution™))))))) ; end cond, condiiam 
defun 


(defun check-direct-path (pl p2 current) 
(let* ((edge-t (cons pl (list p2))) 
(hndg-t (angle edge-t))) 
(cond ((inside-rb hdg-t (segment-critical-b *first-win*) 
(segment-dual-b *first-win*) ) 
(D-cost “pi pZ nile nw) 
((inside-pl hdg-t (segment-critical-p *first-win*) 
(segment-dual-p *first-win*) ) 
(p-cost (pl p2 “*first-wan-s) 
((inside-pl hdg-t (segment-critical-l *first-win*) 
(segment-dual-l *first-win*) ) 
Hal} 
((inside-rb hdg-t (segment-critical-r *first-win*) 
(segment-dual-r *first-win*) ) 
nil) 
(t 
(1-cost pl p2 *first—win 3p) 


(defun store-path (init) 
(let ((current init) 


2 


(path-cost Q) 
(temp) (path-list) ) 


(loop 
(setf temp (make-path 
:region (segment-region current) 
spoint (segment-exit-point current) 
:sheading (segment-heading current) 
:cost (segment-cost current) )) 


(setf path-cost (+ path-cost (segment-cost current) )) 
(setf path-list (append path-list (list temp) )) 
(setf£ current (segment-next-win current) ) 
(1£ (null current) 
(return) )) 
(append (list path-cost) path-list))) 


(defun print-best-path (path-list) 
(let* ((cost (first path-list) ) 


(segments (rest path-list))) 
(dolist (element segments) 
(pprint pee a ie) 
(OPrAnt | port.) (pprint (path-point element) ) 
(pprint "“heading:") (pprint (path-heading element) ) 


(Oprintk sCoset. ~ ) (pprint (path-cost element) ) ) 
(ePrints sa===---—-—— Ee) 

(Peri ntaeparn—-cost:“”) (pprint cost) 
(oor er} 


(defun print-stored-costs (path-list) 


we Ye Ye Ye Yo Yo Ve 


(pprint t eee eee eee nr) 
(pprint "alternate path costs:") 
(ppr int "“---------- ") 


(cond (path-list 
(dolist (path path-list) 
(setf segments (rest path)) 
(dolist (element segments) 
(pprint a — 
(ppEInte iPoine. 1) (pprint (path-point element) ) 
(pprint “heading:") (pprint (path-heading element) ) 


(pprines Cost: ") (pprint (path-cost element) )) 
JESS OE LG Nt ") 

(pprint “path-cost a (pprintee( far stepath) ) )» 

it 


(pprint "no improvements found") ))) 
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; Filename: anneal.cl 

; By: HILTON, Cary A. 

; Date: May 91 

Function: Performs path annealing. 


; 1. detours around path segments 
; with an impermissible heading 


2. performs random path annealing 


3. uses A* search from a detour start point 
to a detour goal point 


4. detour path cannot pass through more than 
3 regions 
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(defvar *detour-rlist* nil) 
(defvar *detour-start* nil) 
(defvar *detour~-goal* nil) 
(defvar *upstream-regions* nil) 
(defvar *downstream-regions* nil) 
(defvar *droot* nil) 


(defvar *save-link-begin*) 
(defvar *save-link-end*) 
(defvar *restore-link-begin*) 
(defvar *restore-link-end*) 


(defun find-region-in-winlist (target-region) 
(let ((current-window *first-win*) ) 

(loop 

(if (eq (segment-region current-window) 

target-region) 
(return) 
(setf current-window (segment-next-win 
current-window) ) ) ) 

current-windovw) ) 


(defun find-index-in-winlist (win-num) 
(let ((current-window *first-win*) ) 
(loop 
(if (eq (segment-index current-window) 
win-num) 
(return) 
(setf current-window (segment-next-win 
current-window) ) ) ) 
(segment-region current-window) )) 


(defun detour (avoid-region) 
(let ((start-region) (goal-region) 
(detour-start) (detour-goal) 
(avoid-edge) 
(window) ) 
(setf window (find-region-in-winlist avoid-region) ) 
(setf detour-start (segment-exit-point 
(segment-prior-win window) ) ) 
(setf detour-goal (segment-exit-point 
(segment-next-win window) ) ) 
(setf start-region (segment-region window) ) 
(setf goal-region (segment-region 
(segment-next-win window) ) ) 
(setf avoid-edge (segment-frontier window) ) 
(setf *upstream-regions* (get-upstream avoid-region *rlist*) ) 
(setf *downstream-regions* (get-downstream avoid-region 
are list*) ) 
(setf *detour-rlist* (anneal-search detour-start 
detour-goal 
start-region 
goal-region 
avoid-edge) ))) 


(defun get-downstream (region rlist) 
(let (downstream) 
(loop 
(ee (Or “inull pelist) 
(eq region (first rlist))) 
(return) ) 
(setf downstream 
(append downstream (list (first rlist)))) 
(setf rlist (rest rlist))) 
downstream) ) 


(defun get-upstream (region rlist) 
(loop 
(if (or (null rlist) 
(eq region (first rlist))) 
(return) 
(setf rlist (rest rlist)))) 
(rest rlist)) 


(defun anneal-search (start goal 
start-region 
goal-region 
avoid-edge) 
(let* ((active-region start-region) 
(prior-edge (cons start (list start))) 
(current-edge) (node-triple) 
(active-node) (active-node-cost) 
(est-to-goal) (cost-tot 0) (temp-node) 
(prior-node start) (prior-region start-region) 
(prior—-cest 0} 
(back-path (list start-region)) (rlist)) 
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; (pprint “detour stare ypome:) 
;(pprint start) 
; (pprint “detour start region: | 
; (pprint start—-reguiem, 
; (pprint "detour qoal point.) 
;(pprint goal) 
;(pprint “detour goal region:") 
; (pprint goal-region) 
(set£ *droot* nil) 
(setf current-node (make-node 


:point start 
:edge nil 
:region start-region 
:chain nae 
-COSE 0 
:cost-est 0 
sprioTr nil 
snext nil) ) 


(loop 
(dolist (element (polygon-alist (eval active-region) ) ) 
(unless (or (> (length (node-chain current-node)) 2) 
(and (eq element prior-region) 
(not (eq start-region goal-region) ) ) 
(virtual-obstacle element) 
(and (memberp element (node-chain current- 
node) ) 
(not (eq start-region goal-region) )) 
(memberp element *downstream-regions*) ) 


(setf current-edge (find-edge (eval active-region) 
(eval element) ) ) 
(setf current-edge 
(cons (eval (first current-edge) ) 
(list (eval (second current-edge))))) 
(unless (or (null (first current-edge) ) 
(null (valid-entry prior-edge current-edge 
active-region) ) 
(equal current-edge avoid-edge) ) 


(setf node-triple (append 
(list (mid-point (first current-edge) 
(second current-edge) ) ) 
(list (first current-edge) ) 
(list (second current-edge) ) )) 


(dolist (active-node node-triple) 
(setf active-node-cost 
(+ (w-distance prior-node active-node 
(polygon-weight (eval active-region) ) ) 
prior-cost) ) 
(draw-dotted (cons active-node (list prior-node) ) ) 
(setf est-to-goal (distance active-node goal) ) 
(setf cost-tot est-to-goal) 
; (setf cost-tot (+ active-node-cost est-to-goal) ) 
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(setf temp-node (make-node 


Deine active-node 
:edge current-edge 
>region element 
Pelvain back-path 
sCOSt active-node-cost 
;cost-est (CHER MEL OIE 
“pL Lor jo wal: 
next maid.) ) 
(setf£f *droot* (insert-node temp-node *droot*))) ; end 


dolist 


))) send unless, unless, unless, dolist 


(VEG at OOt * ) 
(return) 
(setf current-node *droot*) ) 
(setf£ back-path (append (node-chain current-node) 
(list (node-region current-node) ))) 
(setf prior-cost (node-cost current-node) ) 
(1£ (memberp (node-region current-node) *upstream-regions*) 
(return) ) 
(setf prior-edge (node-edge current-node) ) 
(setf£ *droot* (node-next *droot*) ) 
(setf prior-region active-region) 
(setf£ prior-node (node-point current-node) ) 
(set£f active-region (node-region current-node))) ; end loop 


(if (eq (node-region current-node) goal-region) 
(draw-dotted (cons (node-point current-node) 
(last. goal) )')) 
(setf rlist (append (node-chain current-node) 
(list (node-region current-node) ))) 
(1£f (memberp (first (last rlist)) 
*upstream-regions*) 
rlaeSte 
na)))))) 


(defun detour-at (detour-segment) 
(let ((detour-rlist) (detour-winlist) 

(detour-start) (detour-goal) 

(index) (save-frontier) (save-length) 

(detour-region detour-segment) ) 

(cond (detour-segment 
(setf detour-rlist (detour detour-region) ) 
(cond (detour=rlist 
(set£ detour—-start (find-region-in-winlist detour- 


region) ) 
(setf detour-goal (find-region-in-winlist 
(first (last detour-rlist)))) 
(setf save-frontier (segment-frontier detour-goal) ) 
(setf save-length (segment-frontier-length detour- 
goal) ) 
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(setf *detour-winlist* 
(change-struct 
(segment-exit-point detour-start) 
(segment-exit-point detour-goal) 
detour-riasey 
(double-link (first *detour-winlist*) (rest *detour- 
winlist*) ) 
(setf (segment-next-win (first (last *detour-winlist*))) 
nil) 
(setf£ (segment-frontier (first (last *detour-winlist*))) 
save-frontier) 
(setf£f (segment-frontier-length (first (last *detour- 
winlist*))}) 
save-length) 
(setf *save-link-begin* detour-start) 
(setf *restore-link-begin* (Segment-prior-win detour- 
start) ) 
(set£ (segment-next-win *restore-link-begin*) 
(second *detour-winlist*) ) 
(setf£ (segment-prior-win (second *detour-winlist*) ) 
*restore-link-begin*) 
(unless (null (Segment-next-win detour-goal) ) 
(setf£ *save-link-end* detour-goal) 
(setf£ *restore-link-end* 
(segment-next-win detour-goal) ) 
(setf£ (segment-prior-win 
*restore-link-end*) 
(first (last *detour-winlist*) )) 
(setf£ (segment-next-win (first (last *detour-winlist*) )) 
*restore-link-end*) ) 
(renumber-indices))))))) 


(defun renumber-indices () 
(let ((current *init-win*) (index 0)) 

(setf *first-win* 
(segment-next-win *init-win*)) 

(loop 
(set£ (segment-index current) index) 
(setf current (segment-next-win current) ) 
(incf index) 
(1 (null current) 

(return))))) send loop, let, defun 


(defun random-region () 
(let ((detour-num) (detour-region) 
(win-num) ) 
(setf *win-num* 
(segment-index (first (last *winlist*)))) 
(setf detour-num (random *win-num*) ) 
(1f (< detour-num 1) 
(setf detour-num 1)) 
(find-index-in-winlist detour-num) ) ) 


(defun restore-path () 
(setf (segment-next-win *restore-link-begin*) 
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*save-link-begin*) 

(setf (segment-prior-win *save-link-begin*) 
*restore-link-begin*) 

(if *restore-link-end* 
(setf£f (segment-prior-win *restore-link-end*) 

*save-link-end*) ) 

(setf (segment-next-win *save-link-end*) 
*restore-link-end*) 

(setf£ *first-win* (segment-next-win 

*init-win*) ) 
(renumber-indices) 
nil) 
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; Filename: converge.cl 

, By: HILTON, Caryee. 

; Date: May 91 

5 Funct ton. Control structure for. the golden ratio searen 
routine. 


: 1) setf tolerances which determine when to 
; stop the search 


; 2) computes current path cost 
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(defvar *edge-tolerance* 12) ; edge length during golden 

; ratio search 
(defvar *point-tolerance* 6) ; crossing point displacement 

; used in "converge" 
(defvar *precision* ; halt search if improvement of 

; current cost is less than this 
value 
(setf *lcount* 6) ; Max number of outer loops 
allowed 


; during "converge" 
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(defun converge (current) 
(let ((anchored 1) (count 0) 
(current-cost 0) (prev-cost *infinity*) ) 
(Loop 
(setf anchored 1) 
(loop 
(unless (or (null (Segment-next-win current) ) 
(= (segment-frontier-length current) 0)) 
; (1f (and (= (first (segment-exit-point current) ) 
; (first (segment-exit-point (segment-next-win 
current) ))) 
; (= (second (segment-exit-point current) ) 
; (second (segment-exit-point (segment-next-win 
CUEYTENE)) ))) 
; (snip (segment-next-win current) ) ) 
(adjust-crossing current) 
(1£ (> (segment-displ current) *point-tolerance*) 
(setf anchored 0))) ; end unless 


(if (null (segment-next-win current) ) 


(return) 
(setf current (segment-next-win current)))) ;end loop 
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Ginciweount) 
(setf prev-cost current-cost) 
(setf current-cost (calc-path-cost *first-win*) ) 
(pprint “path cost") (pprint current-cost) 
(if (or (plusp anchored) 
(trivial-improvement 
prev-cost current-cost) 
(< *lcount* count) ) 
(return) 
(setf current *first-win*) )) 
(Pprinte. “Leount =" ) 
(pprint count) )) 


; end loop 


(defun trivial-improvement (cl c2) 
lees ((aiff (— cl c2))) 
Gt (OFS CZ Cl) 
([ (7 Gaett oi) 
*precision*) ) 
t 
Mal) }) 


(defun calc-path-cost (element) 
(let ((tot 0)) 
(loop 


(setf tot (+ tot (segment-cost element) )) 
(setf element (segment-next-win element) ) 
(if (null element) 
(return) )) 
Oc) ) 


"snip" removes a window from 
; the window list. It is 


; not used in current version. 


(defun snip (element) 
foprint “snipping: ") 
(pprint (segment-region element) ) 
(if (eq element *first-win*) 
(setf *first-win* (segment-next-win *first-win*) )) 
(if (not (null (segment-prior-win element) ) ) 
(setf (segment-next-win (segment-prior-win element) ) 
(segment-next-win element) )) 
(if (not (null (segment-next-win element) ) ) 
(setf (segment-prior-win (segment-next-win element) ) 
(segment-prior-win element) ))) 
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Filename: costf.cl 
By: HILTON, Cary A. 
Date: May 91 
Function: Contains cost functions for all possible 


4, 

ld 

s 

; combinations of heading ranges between 
; two regions. 
td 

CG 


1. gets pre-point, pre-range, frontier, 
post-point, post-range, current, 
then branches to appropriate cost function. 
2. returns cost, crossing point, and heading range 
ombination 


ta 
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(defvar *interval* 3) ; used to space points along a segment. 


(defun costf (from pre-range edge to post-range current loop-flag) 
(cond ((or (eq pre-range _ 'Ss) 
(eq post-range 's)) 
(hi-search from edge to current loop-flag pre-range post- 
range) ) 
((eq pre-range 'i) 
(cond ((eq post-range 'i) 
(ii-search from edge to current loop-flag) ) 
((eq post-range 'p) 
(ip-search from edge to current loop-flag) ) 
((eq post-range 'b) 
(ib-search from edge to current loop-flag)))) 
((eq pre-range 'p) 
(cond ((eq post-range '1i) 
(pi-search from edge to current loop-flag) ) 
((eq post-range 'p) 
(pp-search from edge to current loop-flag) ) 
((eq post-range 'b) 
(pb-search from edge to current loop-flag)))) 
((eq pre-range '‘'b) 
(cond ((eq post-range 'i) 
(bi-search from edge to current loop-flag) ) 
((eq post-range 'p) 
(bp-search from edge to current loop-flag) ) 
((eq post-range 'b) 
(bb-search from edge to current loop-flag)))))) 


PRK KKK KK KKK KKK KKK KKK KEK KKK KKK KKK KKK KK KKKEKKKKKK KKK 


; high-cost search 
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(defun hi-search (from edge to current loop-flag pre-range post- 
range) 
lec* ((xl) (yl) Mz) (y2) 
(Pl) (p2yemops) (4) 
(dx) (dy) (ranges) 
(prel) (pre2) (pre3) (pre4) 
(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(cl) (¢c2) (c3) (c4) (c-min)) 


(loop 
(setf pl (first edge) ) 
(setf p4 (second edge) ) 

(set£f xl (first pl)) 
(setf yl (second pl)) 
(setf x2 (first p4)) 
(setf y2 (second p4)) 

(setf dx (fround (/ (- x2 xl) *interval*))) 
(setf dy (fround (/ (- y2 yl) *interval*))) 
(setf£ p2 (cons (+ xl dx) 

(lasto(teyl dy) ))) 
(setf p3 (cons (- x2 dx) 
Getst (=e y2. ay) ))) 


(cond ((eq pre-range 's) 
(setf prel (hi-cost from pl current) ) 
(setf pre2 (hi-cost from p2 current) ) 
(setf pre3 (hi-cost from p3 current) ) 
(setf pre4 (hi-cost from p4 current))) 


(setf prel (i-cost from pl current) ) 

(setf pre2 (i-cost from p2 current) ) 
(setf pre3 (i-cost from p3 current) ) 

(setf pre4 (i-cost from p4 current)))) 


(cond ((eq post-range 's) 
(set spose! (hi-cost pl to (segment-next-win 
GUELTeNt) )) 
(setf post2 (hi-cost p2 to (segment-next-win 
current) )) 


(Setr poses (hi-cost ps3 to (segment-next-win 
current) )) 
(setf post4 (hi-cost p4 to (segment-next-win 
current) ))) 
(t 
(setf£f postl (i-cost pl to (segment-next-win 
current) )) 
(setf£f post2 (i-cost p2 to (segment-next-win 
current) )) 
(Seti posts (1-costwep3 to (segment-next-win 
current) )) 
(setf post4 (i-cost p4 to (segment-next-win 


current))))) 


(sett cim(+ prel post) ) 
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(setf c2 (+ pre2 post2) ) 
(setf c3 (+ pre3 post3)) 
(setf£ c4 (+ pre4 post4)) 
(set£ comin (min cl c2 c3 c4)) 
Cit (<e27e3) 
(setf edge (cons pl (list p3))) 
(setf edge (cons p2 (list p4)))) 
(set£ edge-length (distance (first edge) 
(second edge) ) ) 
(if (or (nulleioop—-filag) 
(< edge-~length *edge-tolerance’®) ) 
(return) )) 


(setf ranges (cons pre-range (list post-range) ) ) 
(cond ((= c-min cl) 
(append (list cl) (list prel) (list postl) (@eist pl} \¢igee 
ranges) ) ) 
((= c-min c2) 
(append (list c2) (list pre2) (list post2) (list p2) (list 
ranges) )) 
((= c-min c3) 
(append (list c3) (list pre3) (list post3) (list p3) (list 
ranges) )) 
(t 
(append (list c4) (list pre4) (list post4) (list p4) 
(list ranges)))))) 


PRR KEKEKEKKEKKKKKKEKKKKKKKEKKEKEKKKKKKKKKKKK 
; 1sotropic-to-isotropic search 


(defun ii-search (from edge to current loop-flag) 
(let* ((crossing-pe} 
(path-leg) 
(prel) (pre2) 
(postl) (post2) 
(cl) (c2)) 
(set£ path-leg (cons from (list to))) 
(set£ crossing-pt (find-int path-leg edge) ) 
(cond ((null eroessing- pe) 
(setf prel (i-cost from 
(first edge) 
current) ) 
(set£ postl (i-cost (first edge) 
to 
(segment-next-win 
current) )) 
(setf pre2 (i-cost from 
(second edge) 
current) ) 
(setf post2 (i-cost (second edge) 
LES) 
(segment-next-win 


current) )) 
(setf cl (+ prel postl)) 
(setf£ c2 (+ pre2 post2)) 


‘Lalsg 00/(0< @ roller) 
(append (list cl) (list prel) 
Petcemeoculj (l1St(first edge)) (list *(i i))) 


(append (list c2) (list pre2) 
(last post2) (list (second edge)) (list '(i i))))) 
Ge 
(setf prel (i-cost from 
@zessing—pt 
current) ) 
(set£ postl (i-cost crossing-pt 
ELS, 
(segment-next-win 
current) )) 
(setf cl (+ prel postl)) 
(append (list cl) (list prel) 
GEictempOcElj n(li1St crossing-pt) (list ‘(i i))))))) 


PRK KK ERK KKK KKK KK EKKKKKKEKEKKKKKKKKKKKK 
; isotropic-to-power search 


(defun ip-search (from edge to current loop-flag) 
meet ( (x1) (yt) 4x2) ~(y2) 
(pl) (p2) (p3) (p4) 
(dx) (dy) 
(prel) (pre2) (pre3) (pre4) 
(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(new-pt (segment-exit-point current) ) 
(cl) (c2) (c3) (c4) (c-min)) 


(loop 
(setf pl (first edge) ) 
(setf p4 (second edge) ) 
(setf xl (first pl)) 
(setf yl (second pl)) 
(setf x2 (first p4)) 
(setf£ y2 (second p4)) 
(setf dx (fround (/ (- x2 xl) *interval*))) 
(setf dy (fround (/ (- y2 yl) *interval*))) 
(setf p2 (cons (+ xl dx) 
(list (+ yl dy)))) 
(setf p3 (cons (- x2 dx) 
iistei-e V2 dy) ))) 
(setf prel (i-cost from pl current) ) 
(setf postl (p-cost pl to (segment-next-win current) )) 
(setf£ pre2 (i-cost from p2 current)) 
(setf post2 (p-cost p2 to (segment-next-win current) )) 
(setf pre3 (i-cost from p3-current) ) 
(set£ post3 (p-cost p3 to (segment-next-win current) )) 
(setf pre4 (i-cost from p4 current) ) 
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(setf post4 (p-cost p4 to (segment-next-win current) )) 
(setf£f cl (+ prel postl)) 
(setf£ c2 (+ pre2 post2)) 
(setf£ c3 (+ pre3 post3) ) 
(setf c4 (+ pre4 post4)) 
(setf£ c-min (min cl c2 c3 c4)) 
(if (<"e2 G3) 
(setf edge (cons pl (list p3))) 
(setf edge (cons p2 (list p4)))) 
(set£f edge-length (distance (first edge) 
(second edge) ) ) 
(if (om (nul litcop-tiaa) 
(< edge-length *edge-tolerance’*) ) 
(return) ) ) 


(cond ((= c-min cl) 
(append (list cl) (list prel) (list postl) (list pl) (list 
Pe) 
((= comin c2) 
(append (list c2) (list pre2) (list post2) (list p2) (list 
(2 pee 
((= c-min c3) 
(append (list c3) (list pre3) (list post3) (list p3) (list 
SE) 
(t 


(append (list c4) (list pre4) (list post4) (list p4) 
(list "Gp oe) 


PRE KEKKEKKKEKEKKKKKEKEKEKEKKEKKKKKKKEKKKKKKK 
; isotropic-to-braking search 


(defun ib-search (from edge to current loop-flag) 
(let* ((x1) (yl) (x2) (y2) 

(pl) (p2) (p3) (p4) 

(dx) (dy) 
(prel) (pre2) (pre3) (pre4) 
(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(cl) (e2) (c3)  (e4)9"(e—mans 

(loop 

(setf pl (first edge) ) 

(setf p4 (second edge) ) 

(seti xi (first “pry 
(setf yl (second pl)) 

(setf x2 (first p4)) 
(setf y2 (second p4)) 

(setf dx (fround (/ (— <2e2 pj eencem a py 
(setf dy (fround (/ (- y2 yl) *interval*))) 
(setf p2 (cons (+ x1 dx) 

(list (+ yimdy)))) 
(setf p3 (cons (- x2 dx) 
(list yy 2edyioaD 
(setf prel (i-cost from pl current) ) 


66 


(setf£ postl (b-cost pl iO 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(setf£f pre2 (i-cost from p2 current) ) 
(setf post2 (b-cost p2 to 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(setf pre3 (i-cost from p3 current) ) 
(set£ post3 (b-cost p3 Le 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(set£f pre4 (i-cost from p4 current) ) 
(setf£f post4 (b-cost p4 to 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(sect Cla prel@post])) 
(setf c2 (+ pre2 post2)) 
(setf c3 (+ pre3 post3)) 
(setf c4 (+ pre4 post4)) 
(setf c-min (min cl c2 c3 c4)) 
(if (< c2 c3) 
(setf edge (cons pl (list p3))) 
(set£ edge (cons p2 (list p4)))) 
(setf£ edge-length (distance (first edge) 
(second edge) ) ) 
CHiaor-scmul ll Loop—f lag) 
(< edge-length *edge-tolerance’®) ) 
(return) )) 


(cond {= c-minwel) 
(append (list cl) (list prel) (list postl) (list pl) (list 
mc B)))) 
((= cC—min C2) 
(append (list c2) (list pre2) (list post2) (list p2) (list 
mee b) ))) 
((= c-min c3) 
(append (list c3) (list pre3) (list post3) (list p3) (list 
mis >) ) ) ) 
(t 
(append (list c4) (list pre4) (list post4) (list p4) 
mast’ (1 Bb) )))))) 


gp RRR KEK KKK KEKKEKKKKKKEKKKKKKKKEKKKKKKKKK 
; power-to-isotropic search 


(defun pi-search (from edge to current loop-flag) 
let Cb y ey) (x2) (y2) 
(pl) (p2) (p3) (p4) 
(dx) (dy) 
(prel) (pre2) (pre3) (pre4) 
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(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(cl) (c2) (C3) ic4 ete —inim) ) 


(loop 
(setf pl (first edge) ) 
(setf p4 (second edge) ) 

(setf£ xl (first pl)) 
(setf yl (second pl)) 
(setf x2 (first p4)) 
(setf£ y2 (second p4)) 

(setf dx (fround (/ (- x2 x1) *interval*))) 
(setf dy (fround (/ (- y2 yl) *interval*))) 
(setf p2 (cons (+ xl dx) 

(list (+ yl ayia) 
(set£ p3 (cons (- x2 dx) 


(list ©" y2.dy) 90> (setf prel (p-cost from pl 


CuLErent | 


(setfi postl (i-cost pl to (segment-next-win current) )) 


(setf pre2 (p-cost from p2 current) ) 


(setf post2 (i-cost p2 to (segment-next-win current) )) 


(setf pre3 (p-cost from p3 current) ) 


(setf post3 (i-cost p3 to (segment-next-win current) ) ) 


(setf£f pre4 (p-cost from p4 current) ) 


(setf£ post4 (i-cost p4 to (segment-next-win current) )) 


(setf cl (+ prel postl)) 
(setf c2 (+ pre2 post2)) 
(setf c3 (Ghepres posts), 
(set£ c4 (+ pre4 post4)) 
(setf c-min (min cl c2 c3 c4)) 
(if (< c2 e335 
(set£f edge (cons pl (list p3))) 
(setf edge (cons p2 (list p4)))) 
(setf£f edge-length (distance (first edge) 
(second edge) ) ) 
(if (or (null loop-flagq) 
(< edge-length *edge-tolerance’®) ) 
(return) ) ) 


(cond ((= c-min cl) 


(append (list cl) (list prel) (list postl) (list pl) (list 


“(po 1) ye) 
((= c-min c2) 


(append (list c2) (list pre2) (list post2) (list p2) (iets 


(p25) 2) 
((= c-min c3) 


(append (list c3) (list pre3) (list post3) (list p3) (list 


es) 
(t 


(append (list c4) (list pre4) (list post4) 
(fst (pele 


PRK KEKE KKK KKK KKEKKKEKEKEKEKKKKKEKKKKKEKE 
; power-to-power search 
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(list p4) 


(defun pp-search (from edge to current loop-flag) 
maicc~ ( (xl) iy ljye(x2) (y2) 
(pl) (p2) (p3) (p4) 
(dx) (dy) 
(prel) (pre2) (pre3) (pre4) 
(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(ell) N( CZ). es) e649) (c=minie) 


(loop 
(setf pl (first edge) ) 
(setf£f p4 (second edge) ) 

(setf xl (first pl)) 
(setf yl (second pl)) 
(setf x2 (first p4)) 
(setf y2 (second p4)) 

(setf dx (fround (/ (- x2 x1) *interval*))) 
(setf dy (fround (/ (- y2 yl) *interval*))) 
(setf p2 (cons (+ xl dx) 

ebsien ey bdy) })) 
(setf£f p3 (cons (- x2 dx) 
(list (- y2 dy)))) (setf prel (e-CcoOst E£rom 
pl current) ) 

(setf£f postl (p-cost pl to (segment-next-win current} ) ) 
(setf pre2 (p-cost from p2 current) ) 
(setf post2 (p-cost p2 to (segment-next-win current) )) 

(setf pre3 (p-cost from p3 current) ) 
(setf£f post3 (p-cost p3 to (segment-next-win current) ) ) 

(setf pre4 (p-cost from p4 current) ) 
(setf post4 (p-cost p4 to (segment-next-win current) ) ) 
(setf cl (+ prel postl)) 
(setf c2 (+ pre2 postZ2)) 

(setf c3 (+ pre3 post3)) 
(setf c4 (+ pre4 post4)) 

(setf c-min (min cl c2 c3 c4)) 
(ite =CZ C3) 

(setf edge (cons pl (list p3))) 

(setf edge (cons p2 (list p4)))) 

(setf edge-length (distance (first edge) 
(second edge) )) 
(ier (null loop—f lag) 
(< edge-length *edge-tolerance’®) ) 
(return) )) 


(cond ((= c-min cl) 
(append (list cl) (list prel) (list postl) (list pl) (list 
‘(p p)))) 
((= comin c2) 
(append (list c2) (list pre2) (list post2) (list p2) (list 
ue Py) 7) 
((= c-min c3) 
(append list c3je( list pres) Vist post3) (list p3) (list 
'(p oe 
1E 
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(append (list c4) (list pre4) (list post4) (list p4) 
(list '(p p))))))) 


PRK KKK KKK KKEKKKEKKKKKKKKKKKEKKEKKKEKKKKE 
; power-to-braking search 


(defun pb-search (from edge to current loop-flag) 
(let*® ( (x0) (YZ 2) 
(pl) (p2) (p3) (p4) 
(dx) (dy) 
(prel) (pre2) (pre3) (pre4) 
(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(cl) (c2) (c3) (c4) (c-min) ) 


(loop 
(setf pl (first edge) ) 
(setf p4 (second edge) ) 

(setf xl (first pl)) 
(setf£ yl (second pl)) 
(setf x2 (first p4)) 
(set£ y2 (second p4)) 

(setf dx (fround (/ (- x2 x1) *interval*))) 
(setf dy (fround (/ (- y2 yl) *interval*))) 
(setf p2 (cons (+ xl dx) 

(list (+ yl dy)))) 
(setf p3 (cons (- x2 dx) 
(liste(s yz iy )is))) 
(setf£ prel (p-cost from pl current) ) 
(set£ postl (b-cost pl to 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(setf pre2 (p-cost from p2 current) ) 
(setf post2 (b-cost p2 Ee 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(setf pre3 (p-cost from p3 current) ) 
(sett posts (b-costs ps Eo) 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(setf pre4 (p-cost from p4 current) ) 
(setf post4 (b-cost p4 ie 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(setf cl (+ prel postl)) 
(setf c2 (+ pre2 post2)) 
(setf c3 (+ pre3 post3)) 
(setf£f c4 (+ pre4 post4)) 
(setf c-min (min cl c2 c3 c4)) 
(1) (orGZ C3} 
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(setf edge (cons pl (list p3))) 
(set£ edge (cons p2 (list p4)))) 
(set£ edge-length (distance (first edge) 
(second edge) ) ) 
(if (or (nulieel@op-fllag) 
(< edge-length *edge-tolerance®) ) 
(return) ) ) 


(cond ((= c-min cl) 
(append (list cl) (list prel) (list postl) (list pl) (list 
eve 5)))) 
((= c-min c2) 
(append (list c2) (list pre2) (list post2) (list p2) (list 
'(p b)))) 
((= c-min c3) 
(append (list -C3jameeStepres) a(listepost>) (list ps) (list 
'(p a 
\ 


(append (list c4) (list pre4) (list post4) (list p4) 
mest “{p b))))))) 


eRKKKKEKKKKKKEKEKEKKKKKKKKKEKKKKKKKKKKKKK 
; braking-to-isotropic search 


(defun bi-search (from edge to current loop-flag) 
mlet*® (({x1l) (yl) (x2) (y2) 

(pl) (p2) (p3) (p4) 

(dx) (dy) 
(prel) (pre2) (pre3) (pre4) 
(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(cl) (c2) (c3) (c4) (c-min)) 

(loop 

(setf pl (first edge) ) 

(setf p4 (second edge) ) 

(setf xl (first pl)) 

(setf yl (second pl)) 

(setf£ x2 (first p4)) 

(set£ y2 (second p4)) 

(setf£ dx (fround (/ (- x2 x1) *interval*))) 
(set£ dym@tround (/ (- y2 yl) *interval*))) 
(set£ p2 (cons (+ xl dx) 

(list (+ yl dy)))) 
(set£ p3 (cons (- x2 dx) 
(iistet- y2 dy) ))} 
(setf prel (b-cost from pl 
(segment-frontier 
(segment-prior-win current) ) 
(segment-frontier current) )) 
(set£ postl (i-cost pl to (segment-next-win current) )) 
(setf£ pre2 (b-cost from p2 
(segment-frontier 
(segment-prior-win current) ) 
(segment-frontier current) )) 
(setf£ post2 (i-cost p2 to (segment-next-win current) )) 
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(setf pre3 (b-cost from p3 
(segment-frontier (Segment-prior-win 
current) ) 
(segment-frontier current) )) 
(setf post3 (i-cost p3 to (segment-next-win current) ) ) 
(set£f pre4 (b-cost from p4 
(segment-frontier (segment-prior-win 
current) ) 
(segment-frontier current) ) ) 
(setf post4 (i-cost p4 to (segment-next-win current) )) 
(setf cl (+ prel postl)) 
(setf c2 (+ pre2 post2)) 
(setf c3 (+ pre3 post3)) 
(setf£f c4 (+ pre4 post4)) 
(setf comin (min cl c2 c3 c4)) 
(if (<"%6e2"c3) 
(setf edge (cons pl (list p3))) 
(setf edge (cons p2 (list p4)))) 
(set£f edge-length (distance (first edge) 
(second edge) ) ) 
(if (or (null loop-flag) 
(< edge-length *edge-tolerance’®) ) 
(return) ) ) 
(cond ((= c-min cl) 
(append (list cl) (list prel) (list postl) (list pl) (list 
(Db) ie 
((= comin c2) 
(append (list c2) (list pre2) (list post2) (list p2) (list 
"(> 72) ae) 
((= comin c3) 
(append (list c3) (list pre3) (list post3) (list p3) (list 
eB ose) h))) 
(t 
(append (list c4) (list pre4) (list post4) (list p4) 
(list "(oma 


PRR ERK KK KK KKK K KKK KKKKKKKKKKKKKKKKKK 
; braking-to-power search 


(defun bp-search (from edge to current loop-flag) 
(let* ((x1) (yl) (x2) (y2) 
(pl) (p2) (p3) (p4) 
(dx) (dy) 
(prel) (pre2) (pre3) (pre4) 
(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(new-pt (segment-exit-point current) ) 
(cl) (c2) (c3) (c4) (c-min) ) 


(loop 

(setf pl (first edge) ) 

(setf p4 (second edge) ) 
(setf xl (first pl)) 

(setf£f yl (second pl) ) 


TZ 


(setf x2 (first p4)) 
(set£ y2 (second p4)) 

(setf dx (fround (/ (- x2 xl) *interval*))) 
(setf dy (fround (/ (- y2 yl) *interval*))) 
(setf p2 (cons (+ xl dx) 

(list (+ yl dy)))) 
(setf p3 (cons (- x2 dx) 
(list (- y2 dy)))) 
(setf prel (b-cost from pl 
(segment-frontier 
(segment-prior-win current) ) 
(segment-frontier current) ) ) 
(setf postl (p-cost pl to (segment-next-win current) )) 
(setf pre2 (b-cost from p2 
(segment-frontier 
(segment-prior-win current) ) 
(segment-frontier current) )) 
(setf£ post2 (p-cost’ p2 to (segment-next-win current) ) ) 
(setf pre3 (b-cost from p3 
(segment-frontier 
(segment-prior-win current) ) 
(segment-frontier current)) ) 
(setf post3 (p-cost p3 to (segment-next-win current) )) 
(setf pre4 (b-cost from p4 
(segment-frontier 
(segment-prior-win current) ) 
(segment-frontier current) )) 
(setf post4 (p-cost p4 to (segment-next-win current) )) 
Setflucl (+ prelipest))) 
(set£f c2 (+ pre2 post2)) 
(Seth Co u(t pres poses) 
(set£ c4 (+ pre4 post4)) 
(set£f c-min (min cl c2 c3 c4)) 
(Tt We 2 cs) 
(setf£f edge (cons pl (list p3))) 
(setf£ edge (cons p2 (list p4)))) 
(set£ edge-length (distance (first edge) 
(second edge) ) ) 
(if (or (null loop-flag) 
(< edge-length *edge-tolerance’®) ) 
(return) ) ) 


(cond ((= c-min cl) 
(appenaun list sci last prel) (list postl) (list pl) (list 
mo) ))) 
((= c-min c2) 
(append (list c2) (list pre2) (list post2) (list p2) (list 
cep) ))) 
((= c-min c3) 
(appeme@m|1Stmcs) (list pre3) “listepost3) (list p3) (list 
‘(Bb p)))) 
(t 
(append (list c4) (list pre4) (list post4) (list p4) 
aust “(b p)))oy)) 
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PR KKK KKEKKKKKKKEKKEKKKKKEKKKKKKKKKKKKKK 
; braking-to-braking search 


(defun bb-search (from edge to current loop-flag) 
(let* ((x1) (yl) (x2) (y2) 
(pl (first (segment-frontier current) )) 
(p4 (second (segment-frontier current) )) 
(p2) (p3) 
(dx) (dy) 
(path~-leg) 
(distl) (dist2) 
(prel) (prez) presyemipre4) 
(postl) (post2) (post3) (post4) 
(edge-length (+ *edge-tolerance* 1)) 
(cl) (c2) ites) (e494), (c-—maim) 
(cond ((equal (third pl) 
(third p4)) 
(setf path-leg (cons from (list to))) 
(setf pl (find-int path-leg edge) ) 
(cond (nui ls, 
(setf distl (+ (i-cost from 
(first edge) 
current) 
(i-cost (first edge) 
[EL®, 
(segment-next-win 
current)))) 
(setf dist2 (+ (i-cost from 
(second edge) 
current) 
(i1-cost (second edge) 
io 
(segment-next-win 
current) ))) 
(if (< distivdistZ 
(setf pl (first edge) ) 
(setf pl (second edge))))) 


(setf prel (b-cost from pl 
(segment-frontier 
(segment-prior-win current) ) 
(segment-frontier current) )) 
(setf£ postl (b=cost pil to 
(segment-frontier current) 
(segment-frontier 
(segment-next-win current) ))) 
(setf cl (+ prel postl)) 
(append (list cl) (list prel) 
(list postl) (list pl) (ist “b 5)))7 
(t 
(Loop 
(setf pl (first edge) ) 
(setf p4 (second edge) ) 
(setf xl (first pl)) 
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(setf yl (second pl)) 
(setf x2 (first p4)) 
(setf£ y2 (second p4)) 

(setf dx (fround (/ (- x2 x1) *interval*))) 
(setf dy (fround (/ (- y2 yl) *interval*))) 
(setf p2 (cons (+ xl dx) 

(lusem( Gevilady.))) ) 


(setf£ p3 (cons 


eee 15) 


CEiscan—my2 Gy) ))) 


(setf£ prel 


Current) ) 


(D-COste irom pi 
(segment-frontier (segment-prior-win 


(segment-frontier current) )) 


(Sete post! (b—-cost pl to 


current) ))) 
(setf pre2 


current) ) 


(segment-frontier current) 


(segment-frontier (segment-next-win 


(b-cost from p2 
(segment-frontier (segment-prior-win 


(segment-frontier current) )) 


(set£ post2 (b-cost p2 ES 


current) ))) 
(setf pre3 


current) ) 


(segment-frontier current) 


(segment-frontier (segment-next-win 


(b-cost from p3 


(segment-frontier (segment-prior-win 


(segment-frontier current))) 


(SGEEVpOSES {(b-CoOst p3 CO 


eurrent)))) 
(setf£ pre4 


current) ) 


(segment-frontier current) 


(segment-frontier (segment-next-win 


(b-cost from p4 


(segment-frontier (segment-prior-win 


(segment-frontier current) )) 


(setf post4 (b-cost p4 to 


current) ))) 


(segment-frontier current) 


(segment-frontier (segment-next-win 


(set£ cl (+ prel postl) ) 
(setf£ c2 (+ pre2 post2)) 
(setf c3 (+ pre3 post3)) 
(setf£ c4 (+ pre4 post4)) 
(setf comin (min cl c2 c3 c4)) 


(ita anee? CS) 


(setf edge (cons pl (list p3))) 
(set£ edge (cons p2 (list p4)))) 
(set£ edge-length (distance (first edge) 


(second edge) ) ) 


(if (or (null loop-flag) 
(< edge-length *edge-tolerance’*) ) 


(return) ) ) 
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(cond ((= c-mingel 
(append (list cl) (list prel) (list postl) (list pl) (list 
' (baba) 
((= c-min c2) 
(append (list c2) (list pre2) (list post2) (list p2) (list 
(DS -b))09 
((= c-min c3) 
(append (list c3) (list pre3) (list post3) (list p3) (list 
"(i by ae 
(t 
(append (list c4) (list pre4) (list post4) (list p4) 
(list “(o9b))) nee 
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; If no intersection, assume 
; heading is on a critical 

; angle and return mid-point 
; of segment as switch-point. 


(defun switch-point (pl p2 hdgl hdg2) 
(let ((rayl (calc-ray pl hdgl1)) 
(ray2 (calc-ray pZ hd@zZ)) 
(int-pt) ) 
(setf int-pt (line-int rayl ray2)) 
(if (null int-pt) 
(mid=point pl p2} 
int-pt))) 


SRK KEKKEKKKEKKEKKEKKKKKKKEKKKKKKKKKKKKKK 


(defun i-cost (pl p2 current) 
(w-distance pl p2 (segment-weight current) )) 


(defun p-cost (pl p2 current) 
(let ((sw-point (switch-point pl p2 (segment-critical-p current) 
(+ pi (segment-dual-p current))))) 
(+ (i-cost pl sw-point current) 
{i-cost sw-point p2 Current) 
, the function switch-point is in “geometry.cl" 


; Note b-cost gets different arguments than i-cost, p-cost 


(defun b-cost (pl p2 edgel edge2) 
(let ((elevl) (elev2) ) 
(cond ((third pl) 
(setf elevl (third pl))) 
((equal pl *start*) 
(setf elevl (find-elev pl (first *rlist*)))) 
(t 
(setf elevl (calc-elev pl edgel)))) 
(cond (Gl Ghinrd 2) 
(setf elev2 (third p2))) 
((equal p2 *goal*) 
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(setf elev2 (find-elev p2 (first (last *rlist*))))) 
(t 
(setf elev2 (calc-elev p2 edge2)))) 
(abs (- elevl elev2)))) 


(defun hi-cost (pl p2 current) 
(if (and (equal (first pl) (first p2)) 
(equal (second pl) (second p2))) 


0 
(+ (w-distance pl p2 (segment-weight current) ) 
mah Balenact =) ) ) 
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Filename: drawf.cl 
By: HILTON, Cary A. 
Date: May 91 


Function: Various drawing functions 


e 
td 
fe 
a, 
tA 
? 
4 


; 1. draws map, path segments, heading ranges 
; 2. computes segment headings, segment costs 
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a, 


(defvar *bad-segments* nil) ; list of path segments 
; with impermissible headings 


(defun draw-regions (rlist) 
(let ((polygon) (vlist)) 
(dolist (element rlist) 
(setf polygon (eval element) ) 
(set£ vlist (polygon-vlist polygon) ) 
(draw-edge (first vlist) 
(first (last vlist))) 
(loop 
(if (null (rest vlist)) 
(return) ) 
(draw-edge (first vlist) 
(second vlist) ) 
(setf£ vlist (rest vlist)))))) 


(defun draw-region (region) 
(let* ((r (eval region) ) 
(vlist (polygon-vlist r))) 
(draw-edge (first vlist) 
(first (last vlist))) 
(loop 
(if (null (rest vlist)) 
(return) ) 
(draw-edge (first vlist) 
(second vlist) ) 
(setf vlist (rest vlist))))) 


(defun draw-edge (xyzl xyz2) 
(let ((pl (eval xyzl)) 
(p2 (eval xyz2))) 
(draw-line-xy *map* (first pl) (second pl) 
(first p2) (second p2) 
swidth 2))) 


(defun draw-dotted (edge) 
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(let ((pl (first edge) ) 
(p2 (second edge) )) 
(draw-line-xy *map* (first pl) (second pl) 
(first p2) (second p2) 
:dashing 2 
swidth 2))) 


(defun draw-start (s) 
(draw-filled-circle-xy *map* (first s) 
(second s) 
3) ) 


(defun draw-goal (g) 
(draw-circle-xy *map* (fir stm) 
(second g) 
3)) 


(defun draw-path-dots (current) 
(loop 
feond ((nulle current) 
(return) ) 
(t 
(draw-filled-circle-xy *map* (first (segment-exit- 


point current) ) 
(second (segment-exit- 


Bernt current)) 2))) 
(setf current (Segment-next-win current) )) ) 


(defun draw-path-lines (current) 
(Loop 
(cond ((null current) 
(return) ) 
(t 
(draw-line-xy *map* 
(first (segment-exit-point 
(Ssegment-prior-win current) )) 
(second (segment-exit-point 
(segment-prior-win current) )) 
(first (Segment-exit-point current) ) 
(second (segment-exit-point current) ) 
:dashing 3))) 
(setf current (Segment-next-win current) ))) 


(defun draw-final (current) 
(let ((path-cost 0) (heading) ) 
(setf *bad-segments* nil) 


(loop 
(cond ((null current) 
(return) ) 
(Cc 
(unless (= (Segment-cost current) 0.0) 
; (eterna ao ae 2) 


(eprint “region"™) 
(pprint (segment-region current) ) 
(setf heading (rad-to-deg (angle 
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(cons (segment-exit-point 
(segment-prior-win current) ) 
(list 
(segment-exit-point current)))))) 
(set£ (segment-heading current) heading) 
(pprint "segment heading") 
(pprint heading) 
(pprint “segment-cost"™) 
(pprint (segment-cost current) ) 
(cond ((illegal-heading current) 
(cond ((> (segment-cost current) 
INP) 
(setf *bad-segments* 
(append *bad-segments* 
(list (segment-region current)))))) 
(setf£ path-cost 
(+ path-cost (segment-cost current) )) 
(draw-line-xy *map* (first (segment-exit-point 
(segment-prior-win current) )) 
(second (segment-exit-point 
(segment-prior-win current) )) 
(first (segment-exit-point current) ) 
(second (segment-exit-point 


wee Be Ze Ve Ve 


CULECNHE)s) 
:dashing 3) 
(draw-ranges (segment-exit-point 
(segment-prior-win current)) current)))) 
(if (null (segment-next-win current) ) 
(draw-ranges *goal* current) ) 
(setf current (segment-next-win current) ) ) 
(pprint “path-cost =") 
(pprint path-cost) 
(cond ((null *bad-segments*) 
(pprint "valid headings for all segments") ) 
ae 
(pprint “impermissible path segments at:") 
(pprint *bad-segments*))))) 


(defun draw-best-path (path) 

(let* ((path-cost (first path) ) 
(path-segments (rest path) ) 
(prior-point *start*) 
(current (first path-segments) ) ) 

(clear *map*) 

(draw-regions *global-region-list*) 

(draw-filled-circle-xy *map* (first *“stare*) 

(second *start*) 


3) 
(loop 
(if (null current) 
(return) ) 
(unless (= (path-cost current) 0.0) 
(pprint "---------------- ") 


(pprint “region") 
(pprint (path-region current) ) 
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(pprint “segment heading") 
(pprint (path-heading current) ) 
(pprint “segment-cost") 
(pprint (path-cost current) ) 
(draw-line-xy *map* (first (path-point current) ) 
(second (path-point current) ) 
(first prior-point) 
(second prior-point) 
:dashing 4) 
(draw-circle-xy *map* (first (path-point current) ) 
(second (path-point current) ) 
3)) ; end unless 
(setf prior-point (path-point current) ) 
(setf£ path-segments (rest path-segments) ) 
(setf current (first path-segments))) ; end loop 
(pprint GO a ee es W) 
(pprint “best path cost:") 
(pprint path-cost) )) 


(defun draw-direct (pl p2 current) 
(draw-line-xy *map* (first pl) (second pl) 
(first p2) (second p2) 
:dashing 3) 
(draw-ranges pl *first-win*) 
(draw-ranges p2 *first-win*) ) 


(defun draw-bounds (element) 
(loop 
(unless (or (null element) 
(= (segment-cost element) 0.0)) 
(draw-ranges (segment-exit-point 
(segment-prior-win element)) element) 
(if (null (segment-next-win element) ) 
(draw-ranges (segment-exit-point 
(segment-prior-win current)) element) 
(setf element (segment-next-win element) ) 
(if (null element) 
(securn)) 3) ) 


(defun draw-circle (current) 
(draw-circle-xy *map* (first (segment-exit-point current) ) 
(second (segment-exit-point current)) 2)) 


(defun draw-dot (current) 
faraw-tilleéd=circle-xy *map* (first (segment-exit-point 
current) ) 
(second (segment-exit-point current)) 2)) 


(defun draw-d (point) 
(draw-Tilled-circle-xy *map* (first point) 
(second point) 2)) 
(defun whiteout (current) 
(let ((pl (segment-exit-point (segment-prior-win current) )) 
(p2 (segment-exit-point current) ) 
(p3 (segment-exit-point (segment-next-win current) ))) 
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(setf (window-stream-foreground-color *map*) white) 
(draw-line-xy *map* (first pl) (second pl) 
(first p2) (second p2)) 
(draw-line-xy *map* (first p2) (second p2) 
(first p3) (second p3)) 
(setf£ (window-stream-foreground-color *map*) black) )) 


(defun draw-current (element) 
(unless (null (Segment-prior-win element) ) 
(draw-line-xy *map* (first (segment-exit-point (Ssegment-prior- 
win element) ) ) 
(second (segment-exit-point (segment-prior-win 
element) ) ) 
(first (segment-exit-point element) ) 
(second (segment-exit-point element) ) 
:dashing 4)) 
(unless (null (segment-next-win element) ) 
(draw-line-xy *map* (first (segment-exit-point element) ) 
(second (segment-exit-point element) ) 
(first (segment-exit-point (segment-next-win 
element) )) 
(second (segment-exit-point (segment-next-win 
element) )) 
:dashing 4))) 


(defun draw-r (pl p2) 
(unless (or (null pl) 
(null (£irst p2Z))) 
(draw-line-xy *map* (first pl) (second pl) 
(first (first p2)) (second (first p2))))) 


(defun qdraw () 
(clear *map*) 
(draw-start *start*) 
(draw-goal *goal*) 
(draw-regions *global-region-list*) 
(draw-path-lines *first-win*) ) 


(defun draw-r (pl p2) 
(unless (or (null pl) 
(null (first p2))) 
(draw-line-xy *map* (first pl) (second pl) 
(first (first p2)) (second (first p2))))) 
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Filename: geometry.cl 
By: HILTON, Cary A. 


Date: May 91 


reasoning functions 


? 

? 

? 

? 

; Function: contains geometric spatial 

? 

: 1. includes functions to determine angles of line 
Ss 


; and critical angles for power, sideslope, and 


limits 

2. determines if a vehicle heading is inside 
a non-permissible heading range 

3. computes line intersections 
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e 


(setf *ray-length* 920) 
(setf *radius* 12) 
(setf *radius2* 8) 

(setf *offset* (/ pi 2)) 


extend ray to intersect edge 
radius for sideslope range icon 
radius for power range icon 
orient north to 0 degrees 


“oe se we *oe 


(defun angle (edge) 
(let ((pl (first edge) ) 
(p2 (second edge) ) 
(offset (/ pi 2)) 
(angle) ) 
Gee (or “eq pl nil) (eq pZ nil)) 
(setf angle nil) 
(setf angle (- offset (atan (- (second p2) (second pl)) 
(SiGe sitemo2)) (farst “pl)')))) ) 
(mod angle *pi2*))) 


(defun angle-between (el e2) 
(sin (abs (- (angle el) (angle e2))))) 


(defun deg-to-rad (degrees) 
(* degrees 0.0174533)) 


(defun rad-to-deg (radians) 
(/ radians 0.0174533)) 


; accepts radian measure 
(defun heading-p-b (slope limit) 
(cond ((and (> slope 0) 
(> slope (abs limit) )) 
(acos (/ (tan limit) (tan slope)))) 
(t 0))) 
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; accepts radian measure 
(defun heading-s (slope limit) 
(cond ((and (> slope 0) 
(> slope limit) ) 
(asin (/ (tan limit) (tan slope) ))) 
(t 0))) 


(defun calc-ray (point angle) 
(let* ((x (+ (* *ray-length* (sin angle)) (first point))) 
(y (+ (* *ray-length* (cos angle)) (Second point) )) 
(end-pt (cons x (list y)))) 
(cons point (list end-pt)))) 


(defun find-region (point) 
(let* ((region-list *global-region-list*) 
(current-region (first region-list) ) 
(inside) ) 
(loop 
(cond ((null region-list) 
(return) ) 
te 
(setf inside (inside-region point current-region) ) 
(if inside 
(return) ) 
(setf region-list (rest region-1list) ) 
(setf current-region (first region-list))))) 
inside) ) 


(defun betweenp (a Db x) 
(or (and (>= x a) (<= x b)) 
(and (>= x b) (<= x a)))) 


(defun on-segment (edge point) 
(let ((xl (first (first edge))) 
(yl (second (first edge) )) 
(x2 (first (second edge) ) ) 
(y2 (second (second edge) )) 
(xt (first point) ) 
(yt (second point))) 
(and (and (betweenp xl x2 xt) 
(betweenp yl y2 yt)) 
(zerop (= (* (> x2 xT yea 
(* (= y2 yl) (- xt x1))))))) 


(defun inside-region (point region) 
(let* ((vlist (polygon-vlist (eval region) )) 
(ray (Calc-ray- point, pi) 
(edge (cons (eval (first (last vlist))) 
(list (eval (first vlist))))) 
(intercept (find-int ray edge) ) 
(cross-count 0) ) 
(unless (null intercept) 
(setf intercept (cons (float (first intercept) ) 
(list (float (second intercept)))))) 
(if (cross-p ray edge) 


(incf cross-count) ) 

(if (equal intercept (butlast (first edge) 1)) 
(incf cross-count) ) 

(loop 

(setf edge (cons (eval (first vlist)) 

(list (eval (second vlist))))) 

(if (cross-p ray edge) 
(incf cross-count) ) 

(if (equal intercept (butlast (first edge) 1)) 
(incf cross-count) ) 

(setf vlist (rest vlist)) 

(if (null (rest vlist)) 
(return) ) ) 

(if (oddp cross-count) 
region 

nil))) 


; accepts radian measure 
(defun draw-ranges (point current) 
(let 
Mio xL et p= Vly (p—x2 i (p—-y2) 
Geax eG viley(r—xZ) (r=y2y) 
exo) lays) —x4) (1-y4) 
(b-x1) (b-yl) (b-x2) (b-y2) 
(slope (segment-slope current) )) 
(unless (null (segment-critical-p current) ) 
(setf p-xl (+ (* *radius2* (sin (segment-critical-p 
current))) (first point) )) 
(setf p-yl (+ (* *radius2* (cos (segment-critical-p 
current))) (second point) )) 
(setf p-x2 (+ (* *radius2* (sin (segment-dual-p current) )) 
werEst point) )) 
(set£ p-y2 (+ (* *radius2* (cos (segment-dual-p current) )) 
(second point)))) 
(unless (null (segment-critical-r current) ) 
(setf r-xl (+ (* *radius* (sin (segment-critical-r current) )) 
EEESE pPOint) ) ) 
(setf r-yl (+ (* *radius* (cos (segment-critical-r current)) ) 
(second point) )) 
(setf r-x2 (+ (* *radius* (sin (segment-dual-r current) )) 
(first point) )) 
(setf r-y2 (+ (* *radius* (cos (segment-dual-r current) )) 
(second point)))) 
(unless (null (segment-critical-l current) ) 
(setf 1-x3 (+ (* *radius* (sin (segment-critical-1l current) )) 
(arrSt. PONE) )) 
(seGi T=y3 (te (* *radius* (cos (segment-critical-1l current) )) 
(second point) )) 
(set£f 1-x4 (+ (* *radius* (sin (segment-dual-1l current) )) 
aEest POiunt ) )) 
(setf£f l-y4 (+ (* *radius* (cos (segment-dual-l current) )) 
(second point)))) 
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(unless (null (segment-critical-b current) ) 
(set£ b-xl (+ (* *radius* (sin (Segment-critical-b current) )) 


(£1 rSt apo oe) 


(setf£ b-yl (+ (* *radius* (cos (segment-critical-b current) )) 


(second point) )) 


(set£ b-x2 (+ (* *radius* (sin (segment-dual-b current) )) 


(first point) )) 


(setf£ b-y2 (+ (* *radius* (cos (segment-dual-b current) )) 


(second point)))) 


(draw-circle-xy *map* 


(first point) 
(second point) 


*radius* 
:dashing 5) 


(cond ((> slope 0) 


(if (> slope (vehicle-angle-p *vehicle*) ) 
(draw-filled-triangle-xy *map* 


nil) 


(firsStepornt) 
(second point) 
pao 

Daye 

p=x2 

p-y2) 


(cond ((> slope (vehicle-angle-s *vehicle*) ) 
(draw-filled-triangle-xy *map* 


(first point) 
(second point) 
r-xl 
av 
r-x2 
1) 


(draw-filled-triangle-xy *map* 


(Go nil)) 


(first pomp 
(second point) 
1l-x3 

l-y3 

1-x4 

loyeun 


(if (> slope (abs (vehicle-angle-b *vehicle*) )) 
(draw-triangle-xy *map* 


Tele) 
GRE o ealeA et) 


(first point) 
(second point) 
b-x1 

Dave 

b=xZ 

Bayz 

:dashing 3) 
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(defun sign (x) 
(cond ((plusp x) 1) 
((minusp x) -1) 
((zerop x) 0))) 


(defun area (pl p2 p3) 
fem tt toect pZ) a(first pl})) (-m(second p3) (second pl) )} 
(7(= {first ps) (first sem)) (— (second p2) (second pil))))) 


(defun slope-and-int (pl p2) 
(let ((xl (first pl)) 
(yl (second pl)) 
(x20 (Liest pZ)) 
(y2 (second p2))) 
(cond ((= xl x2) (list nil x1)) 
(tc we@tst (/ (— y2 yl) (- x2 x1)) 
ee cece oy (= yo yl) (—" x2 x1))))))))) 


(defun find-pt (mbl mb2) 
Pret (tml (first mb1) } 
(b1 (second mb1) ) 
(m2 (first mb2)) 
(b2 (second mb2)) ) 
(cond ((equal ml m2) 
(cond ((notw(= bi bZ)) (list nil nil)) 
Geant tasting) bl) ) )} 
((ecemimemame) (list bl (+ b2 (* ~m2abl)))) 
((eq m2 nil). (list bo2 (+ bi €* ml b2)))) 
(Taehist s(tfround (7 (— b2 bl) (-— ml m2))) 
GErounage<+ bl (* ml (7/7 (— b2 bl) “(= ml m2))))))))))) 


(defun cross-p (el e2) 
(let ((pl (first e1)) 
(p2 (second el) ) 
(p3 (first e2)) 
(p4 (second e2))) 
(if (and (/= (sign (area pl p2 p3)) 
(sign (area pl p2 p4))) 
(/= (sign (area p3 p4 pl)) 
(Sign (area p3 p4 p2)))) 
ie 
nil))) 


; Find intersection of two segments 


(defun find-int (el e2) 
(let ((mb-one) (mb-two) (int) ) 
(cond ((cross-p el e2) 
(setf mb-one (slope-and-int (first el) (second el))) 
(setf mb-two (slope-and-int (first e2) (second e2))) 
(setf int (find-pt mb-one mb-two) ) ) 
(t man) 


; Finds intersection of two lines 
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(defun line-int (el e2) 
(let ((mb-one (slope-and-int (first el) (second el))) 
(mb-two (slope-and-int (first e2) (second e2)))) 
(find-pt mb-one mb-two) )) 


(defun find-valid-crossing (point current) 
(let ((ray-t) (valid-point) ) 
(setf ray-t (calc-ray point (segment-critical-r current) )) 
(setf valid-point (find-int ray-t (segment-frontier current) )) 
(if (null valid-point) 
(setf ray-t (calc-ray point (segment-dual-r current) )) 
(setf valid-point (find-int ray-t (segment-frontier 
current) ))) 
(if (null valid-point) 
(setf ray-t (calc-ray point (segment-critical-l current) )) 
(set£f valid-point (find-int ray-t (segment-frontier 
current) ))) 
(if (null valid-point) 
(setf ray-t (calc-ray point (segment-dual-1l current) )) 
(setf valid-point (find-int ray-t (segment-frontier 
current) ))) 
valid-point) ) 


(defun mark-pt (int) 
(draw-filled-circle-xy *winl* (first int) (second int) 5)) 


(defun draw-line (line) 
(draw-line-xy *winl* (first (first line) ) (second (first line) ) 
(first (second line)) (second (second line) ) 
sbrush-width 2)) 


(defun find-elev (point current) 
(let* ((region (eval current) ) 
(crossing-pts (find-elev-points point region) )) 
(calc-elev point crossing-pts) )) 


(defun find-elev-points (point current) 
(let ((vlist (polygon-vlist current) ) 
(edge) (intl) (int2) 
(elevl) (elev2) 
(rayl (calc-ray point pi)) 
(ray2 (calc-ray point (* pi 2)))) 
(setf£ edge (cons (eval (first vlist)) 
(list (eval (first (last vlist)))))) 
(setf intl (find-int rayl edge)) 
(LE i ntt 
(setf elevl (calc-elev intl edge))) 
(setf int2 (find-int ray2 edge) ) 
(Care 
(setf elev2 (calc-elev int2 edge) )) 
(loop 
(set£ edge (cons (eval (first vlist)) 


(list (eval (second vlist))))) 
(cond (null intl) 
(setf£f intl (find-int rayl edge) ) 
(if intl 
(setf elevl (calc-elev intl edge) )))) 
feondy ( (null ant Z) 
(setf int2 (find-int ray2 edge)) 
(if amt 2 
(setf elev2 (calc-elev int2 edge))))) 
(setf vlist (rest vlist)) 
(if (or (and intl int2) 
(null (rest vlist))) 
(return) )) 
(setf intl (append intl (list elevl))) 
(setf int2 (append int2 (list elev2))) 
eons sinmel (list intZ)))) 


(defun calc-elev (point edge) 
(let* ((end-ptl (first edge) ) 
(end-pt2 (second edge) ) 
(edge cjmtpornt —d) 
(low (min (third end-ptl) (third end-pt2) )) 
(elev-d (abs (- (third end-ptl) 
(Chimdsenad—pt Z) } jae) 
(cond ((= (first end-ptl) 
(first end-pt2) ) 
(setf edge-d (abs (- (second end-ptl) 
(second end-pt2)))) 
(setf point-d (abs (- (second end-ptl) 
(second point))))) 
(t 
(setf£ edge-d (abs (- (first end-ptl) (first end-pt2 
(Ssettepoint—-a (absm(—- (first end-ptl) (first point) ) 
(if (zerop elev-d) 
(third end-pt1l) 
(+ (/ (* point-d elev-d) edge-d) low)))) 


1) 
er) 


(defun calc-displ (pl p2) 
(let ((x1l (first pl)) 
(yl (second pl)) 
(x2 (first p2) ) 
(y2 (second p2))) 
(+ (abs (- xl x2)) 
(abs (- yl y2))))) 


(defun distance (pl p2) 
Glecx (ec loa(first pl) ) 
(yl (second pl)) 
(x2 (test pZ)) 
(y2 (second p2)) 
(x (- x2 x1)) 
(y (- y2 yl))) 
esqrt (7 aigicex) «* v y))))) 


(defun w-distance (pl p2 w) 
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{(let*  ((x1l (fixrstgpi? 
(yl (second pl) ) 
(x2 (first p2)) 
(y2 (second p2)) 
(x (= x2 xi} 
(ly (= y2 yi 
(* (sqrt (+ (5 oxi > ewe?) ) ew))) 


(defun find-edge (rl r2) 
(let (edge) 
(dolist (element (polygon-vlist rl) edge) 
(if (memberp element (polygon-vlist r2)) 
(setf edge (append edge (list element))))) 
(cond ((null (rest edge) ) 
(append edge edge) ) 
((and (equal (first edge) 
(first (polygon-vlist rl1))) 
(equal (second edge) 
(first (last (polygom-viiseyrl)))), 
(reverse edge) ) 
(t 
edgeja7 


(defun mid-point (pl p2) 
(let (Gl (f1¢st pF) 
(yl (second pl)) 
(x2 (first p2)) 
(y2 (second p2))) 
(cond ((or (eq sie; 
(eq nil x2)) (list nil nil)) 
(t 
(list (* 0.5 ( xl x2Z))7@ 0.5 Gey ee 


(defun illegal-heading (current) 
(let ((angle (angle (cons 
(segment-exit-point (segment-prior-win current) ) 
(list (segment-exit-point current))))) 
(cr (segment-critical-r current) ) 
(dr (segment-dual-r current) ) 
(cl (segment-critical-l current) ) 
(dl (segment-dual-l current) ) ) 
(if (or (inside-pl angle cl dl) 
(inside-rb angle cr dr)) 
Ie 
ni) 


(defun inside-pl (angle critical dual) 
(cond ((> critical dual) 
(1£ (and (< angle (- critical .0174)) 
(> angle (+ dual .0174))) 
iE 
nai) ) 
((> dual critical) 
(if (and (> angle (- critical .0174)) 
(< angle (+ dual .0174))) 
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nil 
1G) 
(eee) )) 


(defun inside-rb (angle critical dual) 
(cond ((> critical dual) 
(if (and (> angle (- dual .0174)) 
(< angle (+ critical .0174))) 
Mao 
=) 
((> dual critical) 
(if (and (> angle (+ critical .0174)) 
(< angle (- dual .0174))) 
e 
nil)} 
(er ma i))) 


(defun virtual-obstacle (current) 
(let* ((region (eval current) ) 
(ps-rad (deg-to-rad (polygon-slope region) )) 
(vp-rad (deg-to-rad (vehicle-angle-p *vehicle®*) )) 
(vs-rad (deg-to-rad (vehicle-angle-s *vehicle*) )) 
(critical-p (heading-p-b ps-rad vp-rad) ) 
(critical-s (heading-s ps-rad vs-rad) )) 
mee (= Critazcal—p ecritical-s) 
c 
nil))) 


(defun valid-entry (edge-from edge-to region) 
(let* ((r (eval region) ) 
(omiyemeez) {ptl) (pt2) 
(rotation) 
(slope (deg-to-rad (polygon-slope r))) 
Herm ear)) (cl) (dl) 
(hl) (h2) (hdgl) (hdg2)) 
(cond ((> slope 
(vehicle-angle-s *vehicle*) ) 
(setf pfl (first edge-from) ) 
(setf£ pf2 (second edge-from) ) 
(setf ptl (first edge-to) ) 
(setf£ pt2 (second edge-to) ) 
(setf£ rotation (deg-to-rad (polygon-orientation r))) 
(setf cr (heading-s slope 
(vehicle-angle-s *vehicle*) )) 
(setf dr (mod (+ (- pi cr) rotation) *pi2*)) 
Wsctumuec ls (mod, (+ {—- cr) rotation) *piz*) ) 
(setf dl (mod (+ (- cr pi) rotation) *pi2*)) 
(Semmens (cons pil (list pt2))) 
(secteonzs(cons pi2 (list ptl))) 
(setf hdgl (angle hl)) 
(setf hdg2 (angle h2)) 
(setf cr (+ cr rotation) ) 
(if (or (and (inside-rb hdgl cr dr) 
(inside-rb hdg2 cr dr)) 
vande(insrde—-pl hdgl cl dl) 


Ih 


(inside-pl hdg2 cl dl))) 
nil 


t)) 
ft 
t)))) 
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SKK KKK KK KKK KK KKK KEK KEK KEK KKK KKK KEKK KKK KKKEKKKKKKKKKKKKKKKK KKK KK KKK RK 
; Filename: links2.cl 

feey: HILTON, Cary A. 

; Date: May 91 


; Function: Processes region list into window list 


; 1. converts list into doubly linked 

; Circular List 

; 2. creates '‘segment' structure for use in 
: evaluating window sequences. 

5 3. computes critical headings, 

: iNeed seLoSSing points, 

; edge common to adjacent regions 


SRKKKEKEKKKKKEKKKKKEK KKK KK KKK KEKKKEKRKE KKK KKK KK KKKKEKEKEKKKKKEKKEKEKKKKKKKKKKKKE 


(setf *init* 0) ; initialize edge count: odd points to 
left, even points to right 

fee = print—circle* 1) ; turn off print loop for circular data 
structures (doubly linked lists) 


(defconstant *pi2* (* pi 2)) 


p RK KKKKKEKKKKEKKK KKK KEK KKK KEK KEKE KK KEK KKK KKK EKKKKKEKKEKKEKKKEKKKKKKEK KKK KK 


(defstruct segment 
index 
region 
exit-point 
heading 
Peltor —polnt 
displ 
frontier 
frontier-length 
weight 
cost 
next-win 
prior-win 
Sertical—p 
dual-p 
critical-r 
dual-r 
emitacal—] 
dual-1l 
erit ica l—b 
dual-b 
slope 
orientation) 


(defun memberp (item input-list) 
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(TE Input lass 
(if (eq item 
(first input-list) ) 
t 
(memberp item (rest input-list))))) 


(defun double-link (head tail) 
(unless (null tail) 
(set£ (segment-next-win head) (first tail)) 
(set£f (segment-prior-win (first tail)) head) 
(double-link (first tail) (rest tail)))) 


(defun change-struct (start goal rlist) 
(let* ((win-t) (weight-t) 

(exit-t) (aft-t) (next-t) 
(verts) (edge-t) (hdg-t) (ray-t) 
(r) (r-next) 
(ptl) (pt2) (ecount 0) 
(pwr-rad) (side-rad) (brk-rad) 
(crit-p) (crit-r) (crit-l) (crit-b) 
(epyre(dp) (cr) Geis) 
(cl). (al) (eh eeten) 
(slope-rad) (oriented) ) 


(if rlist 
(setf£ win-t (list (make-segment :index 0 
:region (first. wise) 
-exX1lC=porme start 
:frontier (cons 
start 
(list stairt)))))) 
(loop 
{conde ((nul rise 
(return) ) 
(t 


(setf£ r (eval (first rlist))) 
(setf r-next (eval (second rlist))) 
(incf ecount) 
(cond (r-next 
(setf verts (find-edge r r-next) ) 
(set£ ptl (eval (first verts))) 
(setf£f pt2 (eval (second verts)))) 
(t 
(set£ ptl goal) 
(set£ pt2 goal))))) 
(setf£ edge-t (cons ptl (list pt2))) 
(setf weight-t (polygon-weight r)) 
(setf£f slope-rad (deg-to-rad (polygon-slope r))) 
(setf oriented (deg-to-rad (polygon-orientation r))) 
(setf pwr-rad (vehicle-angle-p *vehicle*) ) 
(set£f side-rad (vehicle-angle-s *vehicle*) ) 
(setf brk-rad (vehicle-angle-b *vehicle*) ) 
{setf crit-p (heading-p-b slope-rad pwr—rad)) 
(setf crit-r (heading-s slope-rad side-rad) ) 
(setf£ crit-l (- crit-r)) 
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(setf crit-b (heading-p-b slope-rad brk-rad) ) 
(setf cp (mod (+ crit-p oriented) *pi2*) ) 
(Setr dem (mode(ta(— crit-p) orientedjm*pi2*)) 
(setf cr (mod (+ crit-r oriented) *pi2*)) 

(if (= crite-r crit-l) 

(setf£ dr (mod (+ crit-r oriented) *pi2*)) 
(Sebmear simocda(ta(— pi crit-r) oriented) *pi2*) jw» 
(setf£f cl (mod (+ crit-l oriented) *pi2*)) 

(if (= crit-r crit-l) 

(set£ dl (med (+ crit-l oriented) *pi2*)) 

(setf£ dl (mod (+ (- crit-r pi) oriented) *pi2*))) 
(setf£ cb (mod (+ crit-b oriented) *pi2*)) 

(setf£ db (mod (+ (- crit-b) oriented) *pi2*)) 
(cond ((inside-rb cb cr dr) 


(setf cb dr) 
(setf db dl))) 
; (cond ((oddp ecount) ; set point at 
frontier endpoint 
; (setf exit-t (first edge-t))) 
; ((evenp ecount) 
; (set£ exit-t (second edge-t)))) 


set point at 
- FrOnt Ler 


(Se©Cr exit—t (mid-point ptl pt2) ; 


midpoint 
(setf temp (make-segment :index ecount 

:region (larst rilist) 

:exit-point exrt—t 
[prlor—poumne exit-t 

:displ (+ *point-tolerance* 1) 
:frontier edge-t 
:frontier-length 
(distance (first edge-t) 

(second edge-t) ) 

sweight weight-t 
:cost nal 

:next-win i@ al b 

'pELOE-Win nil 

:Slope slope-rad 

:orientation oriented 

PCIE LCa Lap aye 

:dual-—p dp 

TeLit Cada Gr 

“auali—x dr 

scritical-l ell 

:dual-1 ell 

seritical=b eb 

:dual-b db) ) 


(setf win-t (append win-t (list temp))) 


(setf rlist (rest rlist)) 
(ae (nul) rlist) 
(return win-t))))) 
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PRR KKK KKK KKK KKK KKK KEK KKK KK KKK KKK KEKE KEK KEK KKKEKKKKKKKKKKK KKK KK KK KKK 
; 

Filename: partition.cl 

By: HILTON, Cary A. 


Date: May 91 


~e ee Ve Ye Ye Wo Ve 


Function: These functions partition the 
current window edge at the 

; points where the cost function 

; changes. 

; 1. The possible cost functions are: 

: DD, Pl, PP, Lp, 2a 


=e 


Jojo Jexbly =e) eye 
"Dp" = power range 
; "i" = isotropic (unrestricted) 
= braking range 


2. performs golden ratio 
search with appropriate cost functions. 


td 


, 
, 
: Wh 
FA 
, 
PRK K KEK KK KK KKK KKK KKK KKK KKK KKK KEK KKK KKK KK KKK KKKKKKKKKKKKKKKKKKEKKEEKK 


(defvar *infinity* 65536) ; represents cost of "infinity" 


(defvar *proot*) ; “root for list iofgtransition 
; points along a frontier 


(defstruct partitaon 
displ 
t- point 
frontier 
range 
next 
przez) 


(defun insert-part (new root) 
(let ((top root) (current root)) 
(cond ((null current) 
(setf£ current new)) 
((< (partition-displ new) (partition-displ current) ) 
(setf (partition-prior current) new) 
(setf (partition-next new) current) 
(setf current new) ) 
(t (loop 
(if (and (>= (partition-displ new) (partition-displ 
current) ) 
(not (null (partition-next current) ))) 
(setf current (partition-next current) ) 
(return) ) ) 
(cond ((< (partition-displ new) (partition-displ 
current) ) 
(setf (partition-next new) current) 
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(set£ (partition-prior new) (partition-prior 


mueeent.) ) 


new) 


(setf (partition-next (partition-prior current) ) 


(setf£ (partition-prior current) new) 
(set£ current top) ) 
(t 
(setf£f (partition-next new) nil) 
(set£f (partition-next current) new) 
(setf (partition-prior new) current) 
(setf£f current top))))))) 


(defun insert-plist (plist) 
(let (root) 
(dolist (element plist root) 


(setf£f root (insert-part element root))))) 


(defun pre-part (current) 
(let* ((point (segment-exit-point (Segment-prior-win current) )) 


(edge (segment-frontier current) ) 
(SCP) Scam) Miecr) (rd) 
(7rCl jaateel!) ieeb)a (rdb) 
(cp-int) (dp-int) (cr-int) (dr-int) 
(el -inte (dil int )ateb-ine) “db-int) 
(plist) ) 
(cond ((or (eq point (first edge) ) 
(eq point (Second edge) )) 
nil) 
(t 
(unless (= (segment-critical-p current) 
(segment-dual-p current) ) 
(setf rep (calc-ray point (segment-critical-p current) )) 
(setf rdp (calc-ray point (segment-dual-p current) )) 
(seti cp-int (cons (find-int rcp edge) ‘(i))) 
(setf dp-int (cons (find-int rdp edge) '(p)))) 
(unless (= (segment-critical-r current) 
(segment-dual-r current) ) 
(setf rer (calc-ray point (segment-critical-r current) )) 
(setf£ rdr (calc-ray point (segment-dual-r current) )) 
(setf£ rel (calc-ray point (segment-critical-l current) )) 
(setf rdl (calc-ray point (segment-dual-1l current) ) ) 
(setf cr-int (cons (find-int rcr edge) ‘'(s))) 
(set£ dr-int (cons (find-int rdr edge) '(i))) 
(setf cl-int (cons (find-int rcl edge) '(1i))) 
(setf dl-int (cons (find-int rdl edge) '(s)))) 
(unless (= (segment-critical-b current) 
(segment-dual-b current) ) 
(setf rceb (calc-ray point (segment-critical-b current) )) 
(setf rdb (calc-ray point (segment-dual-b current) )) 
(setf cb-int (cons (find-int rcb edge) '(b))) 
(setf db-int (cons (find-int rdb edge) '(1i)))) 
(append (list dp-int) (list cr-int) (list dr-int) (list cb-int) 
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(Wist db-int) S(list” dil-int) Mlase el ine ae Ss eee 
ATite aye) 


(defun post-part (current) 
(let* ((point (segment-exit-point (segment-next-win current) )) 
(edge (segment-frontier current) ) 
(rcp) S( edn) srcr sian) 
(rcl) (rdl) (rcb) (rdb) 
(cp-int) (dp-int) (cr-int) (dr-int) 
(cl-int) (dl-int) (cb-int) (db-int) 
(plist) ) 
(cond ((or (eq point (first edge) ) 
(eq point (second edge) )) 
nil) 
Ce 


(unless (= (segment-critical-p (Segment-next-win current) ) 
(segment-dual-p (segment-next-win current) ) ) 
(setf rep (calc-ray point (+ pi (segment-critical-p 
(segment-next-win current))))) 
(setf£ rdp (calc-ray point (+ pi (segment-dual-p 
(segment-next-win current) )))) 
(setf cp-int (cons (find-int rep edge) '(p))) 
(setf£ dp-int (cons (find-int rdp edge) '(1i)))) 
(unless (= (segment-critical-r (segment-next-win current) ) 
(segment-dual-r (segment-next-win current) ) ) 
(setf rcr (calc-ray point (+ pi (Segment-critical-r 
(segment-next-win current))))) 
(setf£ rdr (calc-ray point (+ pi (segment-dual-r 
(segment-next-win current))))) 
(setf rel (cale-ray point (+ pi “segment-ceritical-1 
(segment-next-win current))))) 
(setf rdl (calc-ray point (+ pi (segment-dual-1l 
(segment-next-win current) )))) 
(setf cr-int (cons (find-int rcr edge) '(1i))) 
(setf£ dr-int (cons (find-int rdr edge) '(s))) 
(setf cl-int (cons (find-int rcl edge) '(s))) 
(setf dl-int (cons (find-int rdl edge) '(i)))) 
(unless (= (segment-critical-b (segment-next-win current) ) 
(segment-dual-b (segment-next-win current) )) 
(setf rcb (calc-ray point (+ pi (segment-critical-b 
(segment-next-win current))))) 
(setf rdb (calc-ray point (+ pi (segment-dual-b 
(segment-next-win current))))) 
(setf cb-int (cons (find-int rcb edge) ‘'(i))) 
(setf db-int (cons (find-int rdb edge) '(b)))) 
(append (list cp-int) (list cr-int) (list dr-int) (list cb-int) 
(list db-int) (list “dl-int) (list cl=int) “(list wane. 
int) 7999 e 


(defun part (rl r2) 
(let* ((frontier (segment-frontier rl)) 
(pre-point 


(segment-exit-point (segment-prior-win rl))) 
(e—eorne 
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(first (segment-frontier rl))) 
(post=pornt 
(segment-exit-point r2)) 
(pre-trans-pts (pre-part rl)) 
(post-trans-pts (post-part rl)) 
(init-range) 
(t-list) 
(segment-t) ) 
(setf£ t-list 
(dolist (element pre-trans-pts t-list) 
(unless (null (first element) ) 
(setf temp (make-partition 
:displ (calc-displ (first element) t- 
point) 
st-point (first element) 
:frontier ‘pre 
:range (second element) ) ) 
(setf t-list (append t-list (list temp)))))) 
(set£ t-list 
(dolist (element post-trans-pts t-list) 
(unless (null (first element) ) 
(setf£ temp (make-partition 
aarspl (calc-displ (first element) t- 
point) 
st-point (first element) 
sfrontier ‘post 
7oange (second element) ) ) 
(setf£ t-list (append t-list (list temp)))))) 
(setf temp (make-partition 
:displ oli@e aga 
st-point (second frontier) 
:frontier nil 
:range nil) ) 
(setf£ t-list (append t-list (list temp) )) 
(setf *proot* (insert-plist t-list)))) 


(defun adjust-crossing (current) 
(let* ((rl current) (r2 (segment-next-win current) ) 
(pre-point 
(segment-exit-point (segment-prior-win rl))) 
(t-point (first (segment-frontier rl))) 
(post-point (segment-exit-point r2)) 
(pre-segment (cons pre-point (list t-point) )) 
(post-segment (cons t-point (list post-point) ) ) 
(best-frontier) (frontier-t) (best-range) 
(end-ptl) (end-pt2) (ray-t) 
(pre-range) (post-range) 
(best-crossing) 
(next-crossing) (loop-flag) ) 
(setf pre-range (find-pre-range pre-segment rl)) 
(setf post-range (find-post-range post-segment r2) ) 
(part rl r2) 
Loop 
(setf end-ptl t-point) 


oo 


(setf end-pt2 (partition—t—poine a proon))) 
(setf frontier-t (cons end-ptl (list end-pt2))) 
(setf£ next-crossing (costf pre-point pre-range 
frontier-t 
post-point post-range 
Current 
Wael): ) 


(cond ((or (null best-crossing) 
(< (first next-crossing) (first best-crossing) ) ) 
(setf best-crossing next-crossing) 
(setf best-frontier frontier-t) 
(setf best-range (fifth best-crossing) ))) 
(if (equal (partition-frontier *proot*) 'pre) 
(setf pre-range (partition-range *proot*) ) 
(setf£ post-range (partition-range *proot*) )) 
(setf£ *proot* (partition-next *proot*) ) 
(lf (null *proot*) 
(return) 
(setf t-point (second frontier-t)))) ; end loop 


(setf best-crossing (costf pre-point 
(first best-range) 
best-frontier 
post-point 
(second best~-range) 
current 


t)) 


(cond ((< (first best-crossing) 
(+ (segment-cost rl) 
(segment-cost r2))) 


(setf (segment-prior-point rl) (segment-exit-point rl)) 

(whiteout current) 

(draw-region (segment-region current) ) 

(setf (segment-cost rl) (second best~crossing) ) 

(set£ (segment-cost r2) (third best-crossing) ) 

(setf (segment-exit-point rl) (fourth best-crossing) ) 

(draw-current current) 

(setf£f (segment-displ rl) 

(calc-displ (segment-prior-point rl) 
(segment-exit-point rl1l)))))})) + end cond, let 

defun 


(defun print-plist (current) 

(DprInts pilase) 

(loop 
(pprint (partition-t-point current) ) 
(pprint (partition-range current) ) 
(pprint "---------------- ") 
(setf current (partition-next current) ) 
(if (null current) 

(return) ) )) 
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(defun calc-pre-range (heading current) 
(cond ((inside-pl heading 
(segment-critical-p current) 
(segment-dual-p current) ) 
'p) 

((inside-rb heading 
(segment-critical-r current) 
(segment-dual-r current) ) 

's) 

((inside-pl heading 
(segment-critical-l current) 
(segment-dual-l current) ) 

's) 

((inside-rb heading 
(segment-critical-b current) 
(segment-dual-b current) ) 

"b) 
(eo * 2) 9) 


(defun calc-post-range (heading current) 
(cond ((inside-pl heading 
(+ pi (segment-dual-p current) ) 
(+ pi (Ssegment-critical-p current) )) 
'p) 
((inside-rb heading 
(+ pi (segment-dual-r current) ) 
(+ pi (segment-critical-r current) )) 
's) 
((inside-pl heading 
(+ pi (segment-dual-1l current) ) 
(+ pi (Ssegment-critical-1l current) )) 
's) 
((inside-rb heading 
(+ pi (segment-dual-b current) ) 
(+ pi (Segment-critical-b current) )) 
'b) 
(cla 


(defun find-post-range (segment region) 
(let ((hdg) (range) ) 
(if (eq (first segment) 
(second segment) ) 
(setf hdg 
(angle (segment-frontier 
(segment-prior-win region) ))) 

(setf hdg (angle segment) )) 

(setf range (calc-pre-range hdg region) ))) 


(defun find-pre-range (segment region) 
(let ((hdg) (range) ) 
(if (eq (first segment) 
(second segment) ) 
(setf hdg 
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(angle (segment-frontier region) )) 
(setf hdg (angle segment) ) ) 
(setf range (calc-pre-range hdg region) ))) 
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Filename: start.cl 
By:  HiLTON;, Cary A: 
Date: May 91 


Function: A* search returns list of regions 
connecting start and goal points. 


=e Ws =e ®*e Ze Ve Ve =e Ve =e “we 


tT. heuristvems current costuc: weighted distance to 


: 2. attempts to screen path segments for 
: impermissible headings 


CK KK KKK KKK KKK KKK KK KKK KKK KKK KK KKK KKK KEKE KKK KEKE KKK KKK KKKKK KKK KK 


, 


(defvar *nroot* nil) 


; This structure forms a linked list 

; ordered by cost which determines 

; the next node for expansion during 
; the A* region search. 


(defstruct node 

point 

edge 

chain 

region 

cost 

cost-est 
prror 

next) 


(defun region-search (start goal 
Sstart-region 
goal-region) 
(let* ((active-region start-region) 
(prior-edge (cons start (list start))) 
(current-edge) 
(active-node) (active-node-cost) 
(est-to-goal) (cost-tot 0) (temp-node) 
(prior-node start) (prior-region) (prior-cost 0) 
(back-path (list start-region)) (rlist)) 
mipprant “Start point is in region:") 
;(pprint *start*) (pprint start-region) 
mipprant “goal point is in region:") 
;(pprint *goal*) (pprint goal-region) 
(seti@*nrooe™® nil) 
(setf current-node (make-node 
:point start 
sedge jibe 
:region start-region 


103 


*chagn nil 


OE 0 
:cost-est 0 
Hopciliens nit 
snext nil) ) 


;(pprint “detour-region') {pprint detourercqion) 
(Loop 
(dolist (element (polygon-alist (eval active-region) ) ) 
(unless (or (and (eq element prior-region) 
(not (eq start-region goal-region) )) 
(virtual-obstacle element) 
(and (memberp element (node-chain current- 
node) ) 
(not (eq start-region goal-region) ))) 
(setf current-edge (find-edge (eval active-region) 
(eval element) )) 
(setf current-edge (cons (eval (first current-edge) ) 
(list 
(eval (second current-edge) )))) 
(unless (or (null (first current-edge) ) 
(null (valid-entry prior-edge current-edge 
active-region) ) ) 


(setf active-node (mid-point (first current-edge) 
(second current-—edge) ) ) 
(setf active-node-cost 
(+ (w-distance prior-node active-node 
(polygon-weight (eval active-region) )) 
prior-cost) ) 


(draw-dotted (cons active-node (list prior-node) )) 
(setf est-to-goal (distance active-node goal) ) 


(setf£f cost-tot (+ active-node-cost est-to-goal) ) 
(setf temp-node (make-node 
:point active-node 
:edge current-edge 
:region element 
:chain back-path 
:cost active-node-cost 
;COSE—eSe COSt—ECE 
{DEOL nil 
>next nil) ) 


(setf *nroot* (insert-node temp-node *nroot*))))) -;end 
unless, unless, dolist 


(if (null *nroot*) 
(return) 
(setf current-node *nroot*)) 
(setf back-path (append (node-chain current-node) 
(list (node-region current-—node) )) ) 
(setf prior-cost (node-cost current-node) ) 
(1£f (eq (node-region current-node) goal-region) 
(return) ) 
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(setf£ prior-edge (node-edge current-node) ) 

(setf£ *nroot* (node-next *nroot*) ) 

(setf prior-region active-region) 

(setf prior-node (node-point current-node) ) 
(setf active-region (node-region current-node) ) ) 


(draw-dotted (cons (node-point current-node) 
(list goal) )) 
(setf rlist (append (node-chain current-node) 
(list (node-region current-node) ) ) ) 
(if (eq (first (last rlist)) goal-region) 
rlist 
ma) )) 


PRK KKK KKK KK KKK KK KEKE ERK KEKE EK KEK KKK KKK KK KKK KKK KKEKKKKEKKKKKK KKK KKK 
kkeaeke 


(defun insert-node (new root) 
mice. ((top root) (current root) ) 
;(pprint “inside insert-node") 
(cond ((null current) 
(set£ current new) ) 
((< (node-cost-est new) (node-cost-est current) ) 
(setf£ (node-prior current) new) 
(setf (node-next new) current) 
(setf£ current new) ) 
(cee Clcop 
(act (and (>= (node-cost-est new) (node-cost-est 
current) ) 
(not (null (node-next current)))) 
(set£ current (node-next current) ) 
(return) ) ) 
(cond ((< (node-cost-est new) (node-cost-est 
current) ) 
(setf (node-next new) current) 
(setf£ (node-prior new) (node-prior current) ) 
(setf (node-next (node-prior current)) new) 
(setf (node-prior current) new) 
(setf current top) ) 
(t 
(setf (node-next new) nil) 
(setf£ (node-next current) new) 
(setf (node-prior new) current) 
(setf£f current top))))))) 
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Filename: vehicle.cl 


By: HILTON, Cary A. 


; Date: May 91 


Function: Vehicle model. 


1. vehicle is defined by its power, sideslope and 
braking limits. 


e 
; 2. angles are measured in degrees but converted 
; by the program to radian measures. 


3. default is vehicle-2 


4. to change to another vehicle, enter: 
(setf *vehicle* (vehicle-rad vehicle-x) ) 


CK KKK KKK KKK KKK KK KKK KKK KKK KKEKK KKK KE KK KK KKKEKKKEKKKKKEKKEKKEKEKKEKKKEKEKKKK 


a, 


(defstruct vehicle 
angle-p 
angle-s 
angle-b) 


(defvar vehicle-l (make-vehicle :angle-p 31.0 
sangle-s 17.0 
sangle-b -10.0)) 


(defvar vehicle-2 (make-vehicle :angle-p 26.0 
sangle-s 22.0 
sangle-b -5.0)) 


(defvar vehicle-3 (make-vehicle :angle-p 31.0 
sangle-s 17.0 
:angle-b -5.0)) 


(defvar vehicle-4 (make-vehicle :angle-p i. 6 
:angle-s 9.0 
sancgle-b 9 =8.0)) 


(defun vehicle-rad (vehicle) 
(make-vehicle :angle-p (deg-to-rad (vehicle-angle-p vehicle) ) 
sangle-s (deg-to-rad (vehicle-angle-s vehicle) ) 
sangle-b (deg-to-rad (vehicle-angle-b vehicle) ))) 
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Filename: map2.cl 
by: HILTON, Cary A. 


Date: May 91 


; Function: Contains map data. 
‘ 1. defines vertices, edges, and polygons within 

; the map boundaries 

: 2. specifies which vertices and edges each polygon 
: is made from 

; 3. contains adjacency list for polygons 


SKK KK KKK KKK KKK KK KKKK KK KEK KKK KKK KK KKK KKKEKKKKKKKKEKKKKKKKKKKEKKKKEKEK 


; This map closely resembles the map used 
5 by Ron Ross. 


KKEKKEKKKKKKKKKKKEKKKKEKKEKKK KKK KKK KEK KEKE KEKE KKK KKK KEKKKKKKKKEKEKEKKEESE 


(defstruct edge 
pc i 
pt2) 


(defstruct polygon 
wlist 
slope 
orientation 
weight 
alist) 


PRK KKK KEKKKEK KKK KKK KEK KK KKK KEK KKK KK EK EK KKK KEKE KEK KKK KEKKEKKKEKKKKKKKK KK 


; Global vertex list 


(setf£ *global-vertex-list* ‘(vl v2 v3 v4 v5 v6 v7 v8 v9 
aoe val vilZewL Sel 4 yor V6 vid visu 19S 
wom wel Vecevcoevicd V2o V26 v2i vZ28 v29 
v30 v31 v32 v33 v34 v35 v36 v37 v38 v39 
v40 v41 v42 v43 v44 v45)) 


pK KK KKK KKK KKK KKK KKK KKK KKK KKK KEK KKK KKK KKK KK KKK KKK KEKKKKKKEKKKKKKKKKK 


; Global polygon list 


fseete gGlobal—regqion-lists "(rl r29—n5 44 ©r5 r6 r7 r8 x9 
meee) TZ Eiger 4 ellos rl6 rl? rls r19 
meadeeresl r22 E25eEl4eees EZ6er2? r28 r2Z29 
Bo ESlersZ2 1350554 ome Goomrs? r3o) ) 
(setf£ gl *global-region-list*) 
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(setf 
(sett 
(setf£ 
(setf 


(setf 
(sett 
(setf 
(setf 


(set£ 
(setf 
(setf 
(setf 


(setf 
(setf 
(setf 
(setf 


(setf 
(set£ 
(setf£ 
(setf 


(setf 
(setf 
(setf 
(setf 


(setf£ 
(setf 
(setf 
(setf 


(setf 
(setf 
(setf 
(setf 


(sett 
(setf 
(setf 
(set£ 


(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 


vi 
v2 
v3 
v4 


AS, 
v6 
07) 
v8 


v9 
v10 
Vi 
Jie 


v13 
v14 
VES 
v16 


vl1l7 
v18 
v19 
v20 


WZ i 
v22 
v23 
v24 


v25 
v26 
Vi 
v28 


v29 
v30 
Ae Al 
v32 


v33 
v34 
Visa 
v36 


Vio 
v38 
v39 
v40 
v4l 
v42 
v43 
v44 
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"(480. 
"(400. 
"(A Z292r 


" (240. 
"(240. 
P(e 2a 
' (idiz: 
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P3520 
"(416. 
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" (400. 
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“ASSZe 
(OZ ee 
" (400. 
“C2245 


U(Z0G:: 
ZS 
"(144. 
"(144. 


HZ Or 
GZ Ok. 
et SS 
Theo: 


MUASo. 
"(2e¢. 
"( 64. 
"( 64. 


ao ., 
(600: 
EC600r 
(60 Ue 
" (600. 
ak oo age 
EAMG 4 
reer. 


i 1 > | ae I Oooo on a Ee) 


OOO 0 Oooo 


OO OOo oo 2 Oooo ooo oo oO OC' Oo © Oooo 


Ge 
224. 
304. 
176. 


S205 
448. 
448. 
320. 


S60R 
SCG 
400. 
400. 


E60: 
224, 
240. 
16. 


i a Bes 
224. 
28 oe 
176: 


48. 
224. 
352: 
76. 


352. 
416. 
416. 
357" 


304. 
464. 
464. 
304. 


Zi 
496. 
496. 
22, 


GOR 
vo0e 
496. 
224. 


ele teke te kekek®@) i a ae) Ooo O&O OOOO Oooo 


Ooo oO © 


ole teke) ap isl <p) ae a) 


Oo°0O 0 


oleate) 


OOO © 


14 
14 
14 
14 


LO WO WO 


OoOooo0o00 © ooo oO LO WH WH 


2) 
52) 
Oy, 
5) ) 


-0)) 
-O)) 
055) 
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1% 
s Loe 
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ato) 
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ui) 
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hon) 


-9)) 
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.0)) 
-0)) 
iO), 


.0)) 
-0)) 
Oe) 
-0)) 


-0)) 
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10) 
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-0)) 
0) 
008) 
.0)) 
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(setf£ v45 
(setf£ rl 
(setf r2 
(set£ x3 
(setf r4 
(setf x5 
(setf r6 
(setf r7 
(setf r8 
(set£ x19 


io 0 496. 0ge0r 0 ):) 


(make-polygon evil st "(we VZ3 vez v2) 

:slope 23.1 

:Orientation 225 

:weight il 

salist “(x3ZeeZ x7 14))) 
(make-polygon sVilast vil9 vs Vv2ev18) 

:Slope M2 

sOrientation 225 

:weight 1 

salist CE ero ae 7 5)\) } 
(make-polygon Sidialche "(vl5 v1l9 vl18 vl14) 

:slope 27.6 

sorientation 225 

:weight A 

:alist '(riserZzere 19) }) 
(make-polygon erst moe l vil vz v22) 

:slope 25a 

:orientation 315 

:weight al 

salist Gren Oar tr .5))-) 
(make-polygon Poe St ail vil? vis v2) 

:Sslope ay 2 

sorientation 315 

:weight al 

salist Wires r6.%2-r11))) 
(make-polygon :vlist mvild=visevi4d vis) 

:slope 2106 

sorientation 315 

:weight 1 

salist ies Tt Lome oat Zu) ) 
(make-polygon avalais "(v24 v23 v3 v4) 

:slope Zor 1 

:orientation 135 

:weight ih 

salist W{2-34 7S ri 10) ) ) 
(make-polygon avd Ste "(v4 v3 v19 v20) 

:slope eZ 

sorientation 135 

:weight 1 

salist ye Gavimpe!! C7 sally 
(make-polygon :vlist mivZz0 vi9 v15 vi6) 

:slope 27.6 
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(setf 


(setf£ 


(setf 


(sett 


(sett 


(setf£ 


(setf 


(sett 


(setf 


rig 


wag 


ell 7 


balls 


r14 


1S 


r16 


rl17 


baal ls: 


:<Orientaticon 135 
:weight il 
salist 
(make~polygon ava St 
:slope again 
:orientation 045 
:weight i 
salist (Ese 
(make-polygon {Vise 
:Slope Laz 
:orientation 045 
:weight it 
fa List ai (2 oi 8 
(make-polygon Visit 
:slope 27.6 
sorientation 045 
:weight di 
salist ve a dual 
(make-polygon svlist 
:slope 0.0 
:orientation 0 
:weight 1 
salist 
(make-polygon aS 
:Slope sya 
sorientation 0 
:weight 1 
salist as 
(make-polygon sViiast 
:Sslope 2.0 
sOrientation 0 
:sweight a: 
salist "(r14 
(make-polygon iy ea Se 
:Slope io ege | 
;orientation 0 
sweight 1 
salist "(ARS 
(make-polygon Sy asi 
:slope Za S 
:Orientation 0 
:weight i 
salist “(ole 
(make-polygon eVilISse 
:Slope Dich 
:orientation 090 
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'(rSearl3s roe) Z ee) 


*(v24 v4 vl v21) 


rides? 14 )e)i) 


"(v4 v20 vl7 vl) 


Tee On Op) 


‘“stv20 av1 6ev13 vi17) 


rigerS r6))) 


‘(vl6 viS vl4 v3) 


“(Eo eeGers riZ))) 


“(V365V32 V29 va3) 


milo rZ 6 118i) )) 


"(v32 v8 v5 v29) 


r16 r19 r27))) 


"(v8 v28 v25 v5) 


migger 20 7128) )) 
“(yZ2o° VLOMNVS Sv2o) 


r3Qe7r21 rZ29))) 


"(v35 v3l V3Zev36)) 


(setf 


(sett 


(setf 


(setf 


(setf 


(set£ 


(setf 


(setf 


(sett 


r19 


nay ALG) 


=? | 


22 


r23 


r24 


m2 


r26 


27 


:weight i 


salist “CES © 
(make-polygon svlist 
:slope 0.0 
:orientation 090 
:weight 1 
salist '(r18 
(make-polygon eVlaise 
:Slope Bese 
:orientation 090 
:weight il 
salist "(r19 
(make-polygon eal Sic 
:slope 21s 
:orientation 090 
:weight 1 
vabasit (2.0 
(make-polygon SC 
:Slope Se 
:orientation 180 
:weight iy 
saiSst P2311. 
(make-polygon Swlast 
:Slope 0.0 
:Orientation 180 
:weight 1 
sa list ee 2 
(make-polygon Pw St 
:Slope Lee 1 
:orientation 180 
:weight 1 
salist (2s 
(make-polygon svlist 
:Slope 2138 
:orientation 180 
:weight iL 
salist '(r24 
(make-polygon ota st 
:Sslope iRorey 
sorientation 270 
:weight i) 
salist ro S 
(make-polygon ov ilast 
:Slope 0 


sorientation 270 


ise 


eS ate 27 yan ya) 


*(v32 v3 ilewe v8) 


AOE. oi 5,))) } 


vi sv27 v2s8 v8) 


eel r24 +16))) 


v2 vizZ v1l0 vZ8) 


imei o 1 7). )) 


mv 35 v34 v30 v31) 


mee E26) ))} 


tyol v30 vo v7} 


m24.527)1%19))) 


"(vz2i vi v6 v26) 


Ore 26) ) ) 


mivi2 v2/ v26 vil} 


omer 2b 29) )) 
'(v30 v34 v33 v29) 


r27 r22 1r14))) 


"(v6 v30 v29 v5) 


(setf 


(setf 


(setf£ 


(set£ 


(setf 


(setf 


(setf 


(setf£ 


(setf£ 


r28 


r29 


E30 


reeyik 


E32 


r33 


r34 


~35 


r36 


sweight 1 


salist (C263 eee eZee) } 
(make-polygon svIlaSE "“(v26 vV6ey5) v25)) 

:slope 182m 

:orientation 270 

:weight a 

salist "(274229 124 ri6) )) 
(make-polygon “Vv LaSite "(vll v26 v25 v9) 

:slope 27.3 

:Orientation 270 

:weight il 

zsalist ' (728 eg08 re 255x117) )) 
(make-polygon Piglet ie *(v10 vl2 vl1l1 v9) 

:slope 0.0 

:orientation 0 

:weight 1 

salist “(r Zi) 629 rit) )) 
(make-polygon evilist "(v45 v37 v38 v39 v34aere 

:slope 050 

:Orientation 0 

:weight I 

salist '(r2aer3i6 r32)) ) 
(make-polygon svlist '(v34 v39 v40 v22 v23) 

:slope 0.0 

:orientation 0 

:weight 1 

salist '(rsitereos. rl 136) )) 
(make-polygon svlist {Vv S4.V2o0V33) 

:Slope 0.0 

:orientation 0 

:weight i 

salist “(e265 oz r 34)))) 
(make-polygon svlist *(v24 v33 v23) 

:slope 0.0 

:Orientation 0 

sweight 1 

salist "457 aoe 35) )i) 
(make-polygon yan Ste '(v36 v33 v24) 

:slope 0:40 

:Orientation 0 

sweight il 

salist "(ri4er34 r37))) ) 
(make-polygon iViase '(v44 v45 v35 v36 v43) 

:slope 0.0 

:Orientation 0 

:weight il 


Ta 


salist el 3 1 OES ee) 


(setf r37 (make-polygon avast ‘(v43 v36 v24 v21 v42) 
:slope OO 
:orientation 0 
sweight 1 
salist “(r36.r35 G10 rsh 
(setf r38 (make-polygon vila St "(v42 v21 v22 v40 v4l1) 
:Slope O30 
sorientation 0 
sweight 1 
salist “(23 Zeer Geer Bi?) ) ) 
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a, 
° 
a, 
, 
4 
a, 
, 
a, 
tA 
? 
? 
7 
C4 
? 
4 
Cd 
4 
a, 
4 
a, 
4 


Date: May 91 
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Filename: map3.cl 


By; HILTON, Caryee 


Function: Contains map data. 


defines vertices, edges, and polygons within 
the map boundaries 

specifies which vertices and edges each polygon 
is made from 

contains adjacency list for polygons 


map3 is identical to map2 except regions 
are weighted 


CKKKKKK KKK KKEKEKK KEK KK KKK KKK KKK KKK KK KK KK KKK KEKKKKKKKEKKKKKKKKKKKKKKKK 


; This map closely resembles the map used 


; by Ron Ross. 


KKKKEKKKKKKKKKKKKKKKKKKKKE KKK KKKKKKKKEKKEKKKKKKKKKKKKKKKK KKK KKK KKK 


(defstruct edge 
ptl 
pt2) 


(defstruct polygon 


vlist 

slope 
orientation 
weight 
alist) 


eK RK KKK K KKK KKK KK KKK KKK KEK KK KKK KKK KKEKKK KKK KKK KKK KK KKKKKKKKKKKKEKKKK 


; Global vertex list 


(set£ *global-vertex-list* ‘'(vl v2 v3 v4 v5 v6 v7 v8 v9 


v1l0 vll1 vl2 vl1l3 v14 v15 v16 vl7 v18 vis 
v20 v21 v22 v23 v24 v25 v26 v27 v28 v29 
v30 v3 v32 v33 vV34 Voor Vo6 V3 evse veo 
v40 v4l1l v42 v43 v44 v45)) 


KKK KKEKKKKEK KKK KKK KKK KEK KKEKKEKK KKK KKK KK KKKEKKKKEKKKKKKEKKEKKKKKKKKKKKK 


; Global polygon list 


(setf£ *global-region=-list* “(rl Ve2er3er4 ese eee 


r10 rll ril2 £13 Fri4seio 2elG 21 eee 
r20 r21 r22 123° F249 25 226) 27 eee 
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ESUmEsiers? ~aours4 735 r+369637 r38) ) 
oer gl *gliebal—-regiron-list*) 


RK KK KKK KKK KKK KK KKK KKK KKK KKK KEK KKK KKK KK KEK KKK KKK KKKKKKKKKKKKKKK KKK 


(setf vl '(352.0 96.0 14.5)) 
(setf v2 '(480.0 224.0 14.5)) 
(setf v3 ‘(400.0 304.0 14.5)) 
(setf v4 ‘(272.0 176.0 14.5)) 
sett v5 '(240.0-320.0. 9.0)) 
(setf v6 '(240.0 448.0 9.0)) 
Msett vi ‘(112.0 448.0 9.0)) 
(setf v8 '(112.0 320.0 9.0)) 
(setf v9 '(192.0 368.0 27.75)) 
msett vl0 ‘(160.0 368.0 27.75) ) 
(setf vll ‘(192.0 400.0 27.75)) 
(setf v12 '(160.0 400.0 27.75)) 
feeee Vis "(352.0 160.0 35.75) ) 
(setf v14 ‘(416.0 224.0 35.75)) 
Msectf vl5 '(400.0 240.0 35.75)) 
esett v1l6 '(336.0 176.0 35.75) ) 
Mcetr vi? ‘(352.0 112.0 18.0)) 
(setf£ v18 '(464.0 224.0 18.0)) 
(setf v19 ‘(400.0 288.0 18.0)) 
(setf£ v20 ‘(288.0 176.0 18.0)) 
Mett v2l '(352.0 48.0 0.0)) 
(setf£ v22 '(528.0 224.0 0.0)) 
Weett v23 '(400.0 352.0 0.0)) 
(setf v24 '(224.0 176.0 0.0)) 
WMsethr v25 ‘(208.0 352.0 19.5) } 
(setf v26 '(208.0 416.0 19.5)) 
(setf v27 '(144.0 416.0 19.5)) 
(set£ v28 '(144.0 352.0 19.5)) 
(setf v29 '(256.0 304.0 9.0)) 
(setf v30 '(256.0 464.0 9.0)) 
(setf v31 '( 96.0 464.0 9.0)) 
(setf v32 '( 96.0 304.0 9.0)) 
(sett vss ‘(288.0 272.0 7 020) ) 
(setf v34 '(288.0 496.0 0.0)) 
(setf v35 '( 64.0 496.0 0.0)) 
(setf£ v36 '( 64.0 272.0 0.0)) 
(setf v37 '( 0.0 790.0 0O.0)) 
(setf v38 '(*x-max* 790.0 0.0)) 
(setf£ v39 '(*x-max* 496.0 0.0)) 
(setf v40 '(*x-max* 224.0 0.0)) 
(setf v4l '(*x-max* O-08 ©.0)) 
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(setf 
(setf 
(setf 
(setf£ 


(setf£ 


(setf 


(setf 


(setf 


(setf 


(setf£ 


(setf£ 


(setf 


v42 
v43 
v44 
v45 


rat 


2 


r3 


r4 


ape, 


r6 


5 eae | 


r8 


352270 OF 00S 0)) 
64.0 0.07 One) 
O0N0;.0. 0.0) ) 
0.0 496.0 0Q.0)) 
(make-polygon ov La Sit “(V38V23 G22 V2) 
:slope 23 
:orientation 225 
>weight 6 
ra list ' (Y382er2 raed) 
(make-polygon avis “(VIS -vemv2 vic 
:Slope 55/2 
:orientation 225 
sweight 4 
salist ' (gl £3Pes eS) )) 
(make-polygon svlist “(vio v19 v1l8 via) 
:slope Zio 
:orientation 225 
:weight 8 
salist “(ELS 12 eee oy) 
(make-polygon :svlist "(v2de vil vz v22) 
:Slope 23 
:Orientation 315 
>weight 6 
salist “(rage se eelOsrs)o) 
(make-polygon 2 Visite "(vi vl? vis v2) 
:Slope ee 
:Orientation 315 
sweight 4 
salist '(r4 Geo EZerl 1) )) 
(make-polygon : Vast "(vl7 vis v14 vie 
:slope 27.6 
:Orientation 315 
:weight at 
salist ' (v5 F13ee3erl2))) 
(make-polygon 7 ae Sit "(v24 v23 v3 v4) 
:slope 2geL 
:Orientation 135 
sweight 6 
salist "(rears rlveld))} 
(make-polygon wi sit "(v4 v3 vl19 v20) 
:Slope a2 
:Orientation 135 
:weight é 
salist “(x7 r9 xr2 rlidea 
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(setf 


(setf£ 


(setf 


(set£ 


(set£ 


(set£ 


(setf 


(set£ 


(setf£ 


r9 


m0 


Te 


ellZ 


ris 


r14 


readies 


r16 


vad Og 


(make-polygon svlast movz0 VIO vis vib} 


:Slope Zeb 

:orientation 135 

:weight 8 

salist Moms. risers 112) ))) 
(make-polygon 7VLISt *(v24 v4 vl v21) 

:slope Fige are | 

:orientation 045 

:weight 6 

Fa lest tomar ry 14) ) } 
(make-polygon :vlist "(v4 v20 vl17 vl) 

:slope da a 

:Orientation 045 

sweight 4 

salist (rel Osta rs 65).) } 
(make-polygon :vlist NeZaoeylGeyi3 v17} 

:Slope 21a6 

:orientation 045 

:weight 8 

:alist "(ries 9 6) ) ) 
(make-polygon x7 acs ite iveGevis > yvl4 vis) 

:Slope 0.0 

:orientation 0 

:weight 1 

valaeSt U(r oe eommes ril2))) 
(make-polygon svilast MVS 6 Vole 9 Vv33) 

:Slope eos 

:orientation 0 

:weight 4 

salist (rs Seems e261 is) )} 
(make-polygon eV Sits iveZ2 V8 vo. V29) 

:Slope 0.0 

;Orientation 0 

:weight i 

salist ‘(ri deekG 2719 r27)))} 
(make-polygon SVE Sa “(VoevZeevzo V5) 

:Slope 1g, 1 

:orientation 0 

:weight 4 

salist (ie homage treo 628) ).) 
(make-polygon evils OCV2S wt Om v25) 

:slope 2 le 

:Orientation 0 

:weight 8 

salist U(r lGees0eae2) 129) )) 


yg 


(setf r18 (make-polygon AV LESE "(v35" vol V3Z egy) 
gas many 


:slope 

:orientation 090 

:weight q 

salist " (r3OMeLO £22 ri14))) 
(setf r19 (make-polygon Vest (Vo ZevSlev? 273) 

:slope 0.0 

sOrientation 090 

:weight i 

salist “(2 lGmeron TZ 3 rl) )) 
(setf r20 (make-polygon Poi glouicie ‘(v7 v27 v28 v8) 

:slope ipotaal 

:Orientation 090 

:weight 4 

salist "(rl9 72 e244 716) )) 
(setf r21 (make-polygon alse '(uZiievi2 vl0 v2Z8) 

:Slope VAR Pe: 

:orientation 090 

sweight 8 

salist "(220 Ta0emco Eh) )) 
(setf r22 (make-polygon Svilast *(v35'V34 vV20Rve)) 

:Slope Lee 

:orientation 180 

:weight 4 

salist "(Crs tezoe 1 8 rZ26) ) } 
(setf r23 (make-polygon eiAalel cue "(volevs0ev6 va) 

:slope 0.0 

:orientation 180 

:sweight 1 

salist (22 ete 27 er ),) } 
(setf r24 (make-polygon visite "(ged Vv iave v26) 

:Sslope Loe 

:Orientation 180 

:sweight 4 

salist "(r23eEZoeezo ©26)-)) 
(setf r25 (make-polygon aS ilatcie evake V2 V 26 el) 

:slope Vag ee: 

:Orientation 180 

sweight 8 

salist "(r2 450) 12 le r29))) 
(setf£ r26 (make-polygon sv Lisit "(v30" v34 v33. vw2Z9) 

:Slope Lae) 

:orientation 270 

sweight 4 

salise "(733 Seer 22 ele) 
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(setf£ 


(setf 


(setf 


(setf£ 


(setf 


(setf 


(setf 


(setf 


(setf 


(setf 


aes | 


r28 


r29 


ze 3 () 


mo | 


S32 


Eos 


r34 


35 


r36 


(make-polygon :vlist 
:slope 
:orientation 270 
:weight il 
salist LEZO 

(make-polygon act. 
:slope Se 
:Orientation 270 
sweight 4 
salist UO 7 

(make-polygon Sv St 
:slope 2723 
:orientation 270 
:weight 8 
salist SE? 2 

(make-polygon avLLst 
:slope 0.0 
:Orientation 0 
:weight 1 
salist He (aaa al 

(make-polygon :svlist 
:Slope 0.0 
sorientation 0 
:weight 1 
salist UGieZ 2 

(make-polygon avast 
:slope 0 
:orientation 0 
sweight cl 
salist ey ae 

(make-polygon vast 
:slope Veo 
:orientation 0 
:weight 1 
salist Perez 6 

(make-polygon :vlist 
:slope 
sorientation 0 
:weight i 
salist 

(make-polygon svlist 
: slope 0.0 
:orientation 0 
:weight 1 
salist "(r14 

(make-polygon vast 
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"(v6 v30 v29 v5) 


eZionet 23 oF 1'5)):).) 


Sev26 v6 v5 v25) 


r29 r24 xr16))) 
“(vill v2Z6 v25 v9) 
morte Yr i] )) ) 
"(v1l0 vl2 vill v9) 
Eeoue2 9.4117) )) 
B(v45 v3? v38 v39 v34 v35) 
Get 32) ) ) 
'(v34 v39 v40 v22 v23) 
r33 rl r38))) 
my S45V2353 V53) 


me2ers4))) 
htv24 v33 vZ3) 


ie) aor 55) ) 


'(v36 v33 v24) 


r34 r37))) 


'(v44 v45 v35 v36 v43) 


:slope 0.0 
sorientation 0 


:weight 1 
salist ''(r3ie eS r37))) 
(setf r37 (make-polygon svlist "(v43 v36 v24 v21 v42) 
:slope 02:0 
:Orientation 0 
:weight 1 
salist "(r36es5 rl10 r38))) 
(setf r38 (make-polygon “Wallies t "(v42 v21 v22 v40 v41) 
:slope G20 
sorientation 0 
sweight 1 
salist "(r32 reamr37))) 
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